<!-- saved from url=(0014)about:internet -->
<html>
<head>
<meta http-equiv="content-type" content="text/html; charset=utf-8" />
<title>RTW Report - AttitudeEKF.c</title>
<script language="JavaScript" type="text/javascript" src="rtwreport_utils.js"></script>
</head>
<body bgcolor="#eeeeee" text="#1122aa" onload="try {if (top) { if (top.rtwFileOnLoad) top.rtwFileOnLoad(window.document); else local_onload();}} catch(err) {};">
<p>
<table border="0" cellspacing="0" cellpadding="6" width="100%%" height="100%%"><tr><td width="100%%" valign="top" bgcolor="#ffffff">
<h4>File: <a href="../AttitudeEKF.c" target="rtwreport_document_frame" id="linkToText_plain">AttitudeEKF.c</a></h4>
<pre id="RTWcode">
<a name="1"><font color="#888888 "><i>    1   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="2"><font color="#888888 "><i>    2   </i></font></a><font color="#117755"><I> * File: AttitudeEKF.c</i></font>
<a name="3"><font color="#888888 "><i>    3   </i></font></a><font color="#117755"><I> *</i></font>
<a name="4"><font color="#888888 "><i>    4   </i></font></a><font color="#117755"><I> * MATLAB Coder version            : 2.6</i></font>
<a name="5"><font color="#888888 "><i>    5   </i></font></a><font color="#117755"><I> * C/C++ source code generated on  : 23-Apr-2017 01:32:45</i></font>
<a name="6"><font color="#888888 "><i>    6   </i></font></a><font color="#117755"><I> */</i></font>
<a name="7"><font color="#888888 "><i>    7   </i></font></a>
<a name="8"><font color="#888888 "><i>    8   </i></font></a><font color="#117755"><i>/* Include files */</i></font>
<a name="9"><font color="#888888 "><i>    9   </i></font></a><font color="#992211">#</font><font color="#992211"><b>include</b></font> <font color="#992211">&quot;AttitudeEKF.h&quot;</font>
<a name="10"><font color="#888888 "><i>   10   </i></font></a>
<a name="11"><font color="#888888 "><i>   11   </i></font></a><font color="#117755"><i>/* Variable Definitions */</i></font>
<a name="12"><font color="#888888 "><i>   12   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_Ji">Ji</a>[9];
<a name="13"><font color="#888888 "><i>   13   </i></font></a><font color="#112266"><b>static</b></font> <a href="rtwtypes_h.html#type_boolean_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_boolean_T');" target="_self"><font color="#1122aa">boolean_T</font></a> <a name="var_Ji_not_empty">Ji_not_empty</a>;
<a name="14"><font color="#888888 "><i>   14   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_x_apo">x_apo</a>[12];
<a name="15"><font color="#888888 "><i>   15   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_P_apo">P_apo</a>[144];
<a name="16"><font color="#888888 "><i>   16   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="var_Q">Q</a>[144];
<a name="17"><font color="#888888 "><i>   17   </i></font></a><font color="#112266"><b>static</b></font> <a href="rtwtypes_h.html#type_boolean_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_boolean_T');" target="_self"><font color="#1122aa">boolean_T</font></a> <a name="var_Q_not_empty">Q_not_empty</a>;
<a name="18"><font color="#888888 "><i>   18   </i></font></a>
<a name="19"><font color="#888888 "><i>   19   </i></font></a><font color="#117755"><i>/* Function Declarations */</i></font>
<a name="20"><font color="#888888 "><i>   20   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_AttitudeEKF_init" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_AttitudeEKF_init');" target="_self"><font color="#1122aa">AttitudeEKF_init</font></a>(<font color="#112266"><b>void</b></font>);
<a name="21"><font color="#888888 "><i>   21   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_b_mrdivide" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_mrdivide');" target="_self"><font color="#1122aa">b_mrdivide</font></a>(<font color="#112266"><b>float</b></font> A[72], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> B[36]);
<a name="22"><font color="#888888 "><i>   22   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_inv" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_inv');" target="_self"><font color="#1122aa">inv</font></a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[9], <font color="#112266"><b>float</b></font> y[9]);
<a name="23"><font color="#888888 "><i>   23   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a href="#fcn_mrdivide" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_mrdivide');" target="_self"><font color="#1122aa">mrdivide</font></a>(<font color="#112266"><b>float</b></font> A[108], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> B[81]);
<a name="24"><font color="#888888 "><i>   24   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a href="#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[3]);
<a name="25"><font color="#888888 "><i>   25   </i></font></a>
<a name="26"><font color="#888888 "><i>   26   </i></font></a><font color="#117755"><i>/* Function Definitions */</i></font>
<a name="27"><font color="#888888 "><i>   27   </i></font></a>
<a name="28"><font color="#888888 "><i>   28   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="29"><font color="#888888 "><i>   29   </i></font></a><font color="#117755"><I> * Arguments    : void</i></font>
<a name="30"><font color="#888888 "><i>   30   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="31"><font color="#888888 "><i>   31   </i></font></a><font color="#117755"><I> */</i></font>
<a name="32"><font color="#888888 "><i>   32   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_AttitudeEKF_init">AttitudeEKF_init</a>(<font color="#112266"><b>void</b></font>)
<a name="33"><font color="#888888 "><i>   33   </i></font></a><b>{</b>
<a name="34"><font color="#888888 "><i>   34   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="35"><font color="#888888 "><i>   35   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> fv5[12] = <b>{</b> 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F, 0.0F,
<a name="36"><font color="#888888 "><i>   36   </i></font></a>    -9.81F, 1.0F, 0.0F, 0.0F <b>}</b>;
<a name="37"><font color="#888888 "><i>   37   </i></font></a>
<a name="38"><font color="#888888 "><i>   38   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="39"><font color="#888888 "><i>   39   </i></font></a>    <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[i] = fv5[i];
<a name="40"><font color="#888888 "><i>   40   </i></font></a>  <b>}</b>
<a name="41"><font color="#888888 "><i>   41   </i></font></a>
<a name="42"><font color="#888888 "><i>   42   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 144; i++) <b>{</b>
<a name="43"><font color="#888888 "><i>   43   </i></font></a>    <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[i] = 200.0F;
<a name="44"><font color="#888888 "><i>   44   </i></font></a>  <b>}</b>
<a name="45"><font color="#888888 "><i>   45   </i></font></a><b>}</b>
<a name="46"><font color="#888888 "><i>   46   </i></font></a>
<a name="47"><font color="#888888 "><i>   47   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="48"><font color="#888888 "><i>   48   </i></font></a><font color="#117755"><I> * Arguments    : float A[72]</i></font>
<a name="49"><font color="#888888 "><i>   49   </i></font></a><font color="#117755"><I> *                const float B[36]</i></font>
<a name="50"><font color="#888888 "><i>   50   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="51"><font color="#888888 "><i>   51   </i></font></a><font color="#117755"><I> */</i></font>
<a name="52"><font color="#888888 "><i>   52   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_b_mrdivide">b_mrdivide</a>(<font color="#112266"><b>float</b></font> A[72], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> B[36])
<a name="53"><font color="#888888 "><i>   53   </i></font></a><b>{</b>
<a name="54"><font color="#888888 "><i>   54   </i></font></a>  <font color="#112266"><b>float</b></font> b_A[36];
<a name="55"><font color="#888888 "><i>   55   </i></font></a>  <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> ipiv[6];
<a name="56"><font color="#888888 "><i>   56   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="57"><font color="#888888 "><i>   57   </i></font></a>  <font color="#112266"><b>int</b></font> j;
<a name="58"><font color="#888888 "><i>   58   </i></font></a>  <font color="#112266"><b>int</b></font> c;
<a name="59"><font color="#888888 "><i>   59   </i></font></a>  <font color="#112266"><b>int</b></font> jAcol;
<a name="60"><font color="#888888 "><i>   60   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="61"><font color="#888888 "><i>   61   </i></font></a>  <font color="#112266"><b>float</b></font> temp;
<a name="62"><font color="#888888 "><i>   62   </i></font></a>  <font color="#112266"><b>float</b></font> s;
<a name="63"><font color="#888888 "><i>   63   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="64"><font color="#888888 "><i>   64   </i></font></a>  <font color="#112266"><b>int</b></font> jBcol;
<a name="65"><font color="#888888 "><i>   65   </i></font></a>  <font color="#112266"><b>int</b></font> kBcol;
<a name="66"><font color="#888888 "><i>   66   </i></font></a>  memcpy(&amp;b_A[0], &amp;B[0], 36U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>));
<a name="67"><font color="#888888 "><i>   67   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 6; k++) <b>{</b>
<a name="68"><font color="#888888 "><i>   68   </i></font></a>    ipiv[k] = (<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>)(1 + k);
<a name="69"><font color="#888888 "><i>   69   </i></font></a>  <b>}</b>
<a name="70"><font color="#888888 "><i>   70   </i></font></a>
<a name="71"><font color="#888888 "><i>   71   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 0; j &lt; 5; j++) <b>{</b>
<a name="72"><font color="#888888 "><i>   72   </i></font></a>    c = j * 7;
<a name="73"><font color="#888888 "><i>   73   </i></font></a>    jAcol = 0;
<a name="74"><font color="#888888 "><i>   74   </i></font></a>    ix = c;
<a name="75"><font color="#888888 "><i>   75   </i></font></a>    temp = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_A[c]);
<a name="76"><font color="#888888 "><i>   76   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 2; k &lt;= 6 - j; k++) <b>{</b>
<a name="77"><font color="#888888 "><i>   77   </i></font></a>      ix++;
<a name="78"><font color="#888888 "><i>   78   </i></font></a>      s = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_A[ix]);
<a name="79"><font color="#888888 "><i>   79   </i></font></a>      <font color="#112266"><b>if</b></font> (s &gt; temp) <b>{</b>
<a name="80"><font color="#888888 "><i>   80   </i></font></a>        jAcol = k - 1;
<a name="81"><font color="#888888 "><i>   81   </i></font></a>        temp = s;
<a name="82"><font color="#888888 "><i>   82   </i></font></a>      <b>}</b>
<a name="83"><font color="#888888 "><i>   83   </i></font></a>    <b>}</b>
<a name="84"><font color="#888888 "><i>   84   </i></font></a>
<a name="85"><font color="#888888 "><i>   85   </i></font></a>    <font color="#112266"><b>if</b></font> (b_A[c + jAcol] != 0.0F) <b>{</b>
<a name="86"><font color="#888888 "><i>   86   </i></font></a>      <font color="#112266"><b>if</b></font> (jAcol != 0) <b>{</b>
<a name="87"><font color="#888888 "><i>   87   </i></font></a>        ipiv[j] = (<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>)((j + jAcol) + 1);
<a name="88"><font color="#888888 "><i>   88   </i></font></a>        ix = j;
<a name="89"><font color="#888888 "><i>   89   </i></font></a>        jAcol += j;
<a name="90"><font color="#888888 "><i>   90   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 6; k++) <b>{</b>
<a name="91"><font color="#888888 "><i>   91   </i></font></a>          temp = b_A[ix];
<a name="92"><font color="#888888 "><i>   92   </i></font></a>          b_A[ix] = b_A[jAcol];
<a name="93"><font color="#888888 "><i>   93   </i></font></a>          b_A[jAcol] = temp;
<a name="94"><font color="#888888 "><i>   94   </i></font></a>          ix += 6;
<a name="95"><font color="#888888 "><i>   95   </i></font></a>          jAcol += 6;
<a name="96"><font color="#888888 "><i>   96   </i></font></a>        <b>}</b>
<a name="97"><font color="#888888 "><i>   97   </i></font></a>      <b>}</b>
<a name="98"><font color="#888888 "><i>   98   </i></font></a>
<a name="99"><font color="#888888 "><i>   99   </i></font></a>      k = (c - j) + 6;
<a name="100"><font color="#888888 "><i>  100   </i></font></a>      <font color="#112266"><b>for</b></font> (i = c + 1; i + 1 &lt;= k; i++) <b>{</b>
<a name="101"><font color="#888888 "><i>  101   </i></font></a>        b_A[i] /= b_A[c];
<a name="102"><font color="#888888 "><i>  102   </i></font></a>      <b>}</b>
<a name="103"><font color="#888888 "><i>  103   </i></font></a>    <b>}</b>
<a name="104"><font color="#888888 "><i>  104   </i></font></a>
<a name="105"><font color="#888888 "><i>  105   </i></font></a>    jBcol = c;
<a name="106"><font color="#888888 "><i>  106   </i></font></a>    jAcol = c + 6;
<a name="107"><font color="#888888 "><i>  107   </i></font></a>    <font color="#112266"><b>for</b></font> (kBcol = 1; kBcol &lt;= 5 - j; kBcol++) <b>{</b>
<a name="108"><font color="#888888 "><i>  108   </i></font></a>      temp = b_A[jAcol];
<a name="109"><font color="#888888 "><i>  109   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[jAcol] != 0.0F) <b>{</b>
<a name="110"><font color="#888888 "><i>  110   </i></font></a>        ix = c + 1;
<a name="111"><font color="#888888 "><i>  111   </i></font></a>        k = (jBcol - j) + 12;
<a name="112"><font color="#888888 "><i>  112   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 7 + jBcol; i + 1 &lt;= k; i++) <b>{</b>
<a name="113"><font color="#888888 "><i>  113   </i></font></a>          b_A[i] += b_A[ix] * -temp;
<a name="114"><font color="#888888 "><i>  114   </i></font></a>          ix++;
<a name="115"><font color="#888888 "><i>  115   </i></font></a>        <b>}</b>
<a name="116"><font color="#888888 "><i>  116   </i></font></a>      <b>}</b>
<a name="117"><font color="#888888 "><i>  117   </i></font></a>
<a name="118"><font color="#888888 "><i>  118   </i></font></a>      jAcol += 6;
<a name="119"><font color="#888888 "><i>  119   </i></font></a>      jBcol += 6;
<a name="120"><font color="#888888 "><i>  120   </i></font></a>    <b>}</b>
<a name="121"><font color="#888888 "><i>  121   </i></font></a>  <b>}</b>
<a name="122"><font color="#888888 "><i>  122   </i></font></a>
<a name="123"><font color="#888888 "><i>  123   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 0; j &lt; 6; j++) <b>{</b>
<a name="124"><font color="#888888 "><i>  124   </i></font></a>    jBcol = 12 * j;
<a name="125"><font color="#888888 "><i>  125   </i></font></a>    jAcol = 6 * j;
<a name="126"><font color="#888888 "><i>  126   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 1; k &lt;= j; k++) <b>{</b>
<a name="127"><font color="#888888 "><i>  127   </i></font></a>      kBcol = 12 * (k - 1);
<a name="128"><font color="#888888 "><i>  128   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[(k + jAcol) - 1] != 0.0F) <b>{</b>
<a name="129"><font color="#888888 "><i>  129   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="130"><font color="#888888 "><i>  130   </i></font></a>          A[i + jBcol] -= b_A[(k + jAcol) - 1] * A[i + kBcol];
<a name="131"><font color="#888888 "><i>  131   </i></font></a>        <b>}</b>
<a name="132"><font color="#888888 "><i>  132   </i></font></a>      <b>}</b>
<a name="133"><font color="#888888 "><i>  133   </i></font></a>    <b>}</b>
<a name="134"><font color="#888888 "><i>  134   </i></font></a>
<a name="135"><font color="#888888 "><i>  135   </i></font></a>    temp = 1.0F / b_A[j + jAcol];
<a name="136"><font color="#888888 "><i>  136   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="137"><font color="#888888 "><i>  137   </i></font></a>      A[i + jBcol] *= temp;
<a name="138"><font color="#888888 "><i>  138   </i></font></a>    <b>}</b>
<a name="139"><font color="#888888 "><i>  139   </i></font></a>  <b>}</b>
<a name="140"><font color="#888888 "><i>  140   </i></font></a>
<a name="141"><font color="#888888 "><i>  141   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 5; j &gt; -1; j += -1) <b>{</b>
<a name="142"><font color="#888888 "><i>  142   </i></font></a>    jBcol = 12 * j;
<a name="143"><font color="#888888 "><i>  143   </i></font></a>    jAcol = 6 * j - 1;
<a name="144"><font color="#888888 "><i>  144   </i></font></a>    <font color="#112266"><b>for</b></font> (k = j + 2; k &lt; 7; k++) <b>{</b>
<a name="145"><font color="#888888 "><i>  145   </i></font></a>      kBcol = 12 * (k - 1);
<a name="146"><font color="#888888 "><i>  146   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[k + jAcol] != 0.0F) <b>{</b>
<a name="147"><font color="#888888 "><i>  147   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="148"><font color="#888888 "><i>  148   </i></font></a>          A[i + jBcol] -= b_A[k + jAcol] * A[i + kBcol];
<a name="149"><font color="#888888 "><i>  149   </i></font></a>        <b>}</b>
<a name="150"><font color="#888888 "><i>  150   </i></font></a>      <b>}</b>
<a name="151"><font color="#888888 "><i>  151   </i></font></a>    <b>}</b>
<a name="152"><font color="#888888 "><i>  152   </i></font></a>  <b>}</b>
<a name="153"><font color="#888888 "><i>  153   </i></font></a>
<a name="154"><font color="#888888 "><i>  154   </i></font></a>  <font color="#112266"><b>for</b></font> (jAcol = 4; jAcol &gt; -1; jAcol += -1) <b>{</b>
<a name="155"><font color="#888888 "><i>  155   </i></font></a>    <font color="#112266"><b>if</b></font> (ipiv[jAcol] != jAcol + 1) <b>{</b>
<a name="156"><font color="#888888 "><i>  156   </i></font></a>      <font color="#112266"><b>for</b></font> (jBcol = 0; jBcol &lt; 12; jBcol++) <b>{</b>
<a name="157"><font color="#888888 "><i>  157   </i></font></a>        temp = A[jBcol + 12 * jAcol];
<a name="158"><font color="#888888 "><i>  158   </i></font></a>        A[jBcol + 12 * jAcol] = A[jBcol + 12 * (ipiv[jAcol] - 1)];
<a name="159"><font color="#888888 "><i>  159   </i></font></a>        A[jBcol + 12 * (ipiv[jAcol] - 1)] = temp;
<a name="160"><font color="#888888 "><i>  160   </i></font></a>      <b>}</b>
<a name="161"><font color="#888888 "><i>  161   </i></font></a>    <b>}</b>
<a name="162"><font color="#888888 "><i>  162   </i></font></a>  <b>}</b>
<a name="163"><font color="#888888 "><i>  163   </i></font></a><b>}</b>
<a name="164"><font color="#888888 "><i>  164   </i></font></a>
<a name="165"><font color="#888888 "><i>  165   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="166"><font color="#888888 "><i>  166   </i></font></a><font color="#117755"><I> * Arguments    : const float x[9]</i></font>
<a name="167"><font color="#888888 "><i>  167   </i></font></a><font color="#117755"><I> *                float y[9]</i></font>
<a name="168"><font color="#888888 "><i>  168   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="169"><font color="#888888 "><i>  169   </i></font></a><font color="#117755"><I> */</i></font>
<a name="170"><font color="#888888 "><i>  170   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_inv">inv</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[9], <font color="#112266"><b>float</b></font> y[9])
<a name="171"><font color="#888888 "><i>  171   </i></font></a><b>{</b>
<a name="172"><font color="#888888 "><i>  172   </i></font></a>  <font color="#112266"><b>float</b></font> b_x[9];
<a name="173"><font color="#888888 "><i>  173   </i></font></a>  <font color="#112266"><b>int</b></font> p1;
<a name="174"><font color="#888888 "><i>  174   </i></font></a>  <font color="#112266"><b>int</b></font> p2;
<a name="175"><font color="#888888 "><i>  175   </i></font></a>  <font color="#112266"><b>int</b></font> p3;
<a name="176"><font color="#888888 "><i>  176   </i></font></a>  <font color="#112266"><b>float</b></font> absx11;
<a name="177"><font color="#888888 "><i>  177   </i></font></a>  <font color="#112266"><b>float</b></font> absx21;
<a name="178"><font color="#888888 "><i>  178   </i></font></a>  <font color="#112266"><b>float</b></font> absx31;
<a name="179"><font color="#888888 "><i>  179   </i></font></a>  <font color="#112266"><b>int</b></font> itmp;
<a name="180"><font color="#888888 "><i>  180   </i></font></a>  <font color="#112266"><b>float</b></font> b_y;
<a name="181"><font color="#888888 "><i>  181   </i></font></a>  <font color="#112266"><b>for</b></font> (p1 = 0; p1 &lt; 9; p1++) <b>{</b>
<a name="182"><font color="#888888 "><i>  182   </i></font></a>    b_x[p1] = x[p1];
<a name="183"><font color="#888888 "><i>  183   </i></font></a>  <b>}</b>
<a name="184"><font color="#888888 "><i>  184   </i></font></a>
<a name="185"><font color="#888888 "><i>  185   </i></font></a>  p1 = 0;
<a name="186"><font color="#888888 "><i>  186   </i></font></a>  p2 = 3;
<a name="187"><font color="#888888 "><i>  187   </i></font></a>  p3 = 6;
<a name="188"><font color="#888888 "><i>  188   </i></font></a>  absx11 = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(x[0]);
<a name="189"><font color="#888888 "><i>  189   </i></font></a>  absx21 = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(x[1]);
<a name="190"><font color="#888888 "><i>  190   </i></font></a>  absx31 = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(x[2]);
<a name="191"><font color="#888888 "><i>  191   </i></font></a>  <font color="#112266"><b>if</b></font> ((absx21 &gt; absx11) &amp;&amp; (absx21 &gt; absx31)) <b>{</b>
<a name="192"><font color="#888888 "><i>  192   </i></font></a>    p1 = 3;
<a name="193"><font color="#888888 "><i>  193   </i></font></a>    p2 = 0;
<a name="194"><font color="#888888 "><i>  194   </i></font></a>    b_x[0] = x[1];
<a name="195"><font color="#888888 "><i>  195   </i></font></a>    b_x[1] = x[0];
<a name="196"><font color="#888888 "><i>  196   </i></font></a>    b_x[3] = x[4];
<a name="197"><font color="#888888 "><i>  197   </i></font></a>    b_x[4] = x[3];
<a name="198"><font color="#888888 "><i>  198   </i></font></a>    b_x[6] = x[7];
<a name="199"><font color="#888888 "><i>  199   </i></font></a>    b_x[7] = x[6];
<a name="200"><font color="#888888 "><i>  200   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="201"><font color="#888888 "><i>  201   </i></font></a>    <font color="#112266"><b>if</b></font> (absx31 &gt; absx11) <b>{</b>
<a name="202"><font color="#888888 "><i>  202   </i></font></a>      p1 = 6;
<a name="203"><font color="#888888 "><i>  203   </i></font></a>      p3 = 0;
<a name="204"><font color="#888888 "><i>  204   </i></font></a>      b_x[0] = x[2];
<a name="205"><font color="#888888 "><i>  205   </i></font></a>      b_x[2] = x[0];
<a name="206"><font color="#888888 "><i>  206   </i></font></a>      b_x[3] = x[5];
<a name="207"><font color="#888888 "><i>  207   </i></font></a>      b_x[5] = x[3];
<a name="208"><font color="#888888 "><i>  208   </i></font></a>      b_x[6] = x[8];
<a name="209"><font color="#888888 "><i>  209   </i></font></a>      b_x[8] = x[6];
<a name="210"><font color="#888888 "><i>  210   </i></font></a>    <b>}</b>
<a name="211"><font color="#888888 "><i>  211   </i></font></a>  <b>}</b>
<a name="212"><font color="#888888 "><i>  212   </i></font></a>
<a name="213"><font color="#888888 "><i>  213   </i></font></a>  absx21 = b_x[1] / b_x[0];
<a name="214"><font color="#888888 "><i>  214   </i></font></a>  b_x[1] /= b_x[0];
<a name="215"><font color="#888888 "><i>  215   </i></font></a>  absx11 = b_x[2] / b_x[0];
<a name="216"><font color="#888888 "><i>  216   </i></font></a>  b_x[2] /= b_x[0];
<a name="217"><font color="#888888 "><i>  217   </i></font></a>  b_x[4] -= absx21 * b_x[3];
<a name="218"><font color="#888888 "><i>  218   </i></font></a>  b_x[5] -= absx11 * b_x[3];
<a name="219"><font color="#888888 "><i>  219   </i></font></a>  b_x[7] -= absx21 * b_x[6];
<a name="220"><font color="#888888 "><i>  220   </i></font></a>  b_x[8] -= absx11 * b_x[6];
<a name="221"><font color="#888888 "><i>  221   </i></font></a>  <font color="#112266"><b>if</b></font> ((<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_x[5]) &gt; (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_x[4])) <b>{</b>
<a name="222"><font color="#888888 "><i>  222   </i></font></a>    itmp = p2;
<a name="223"><font color="#888888 "><i>  223   </i></font></a>    p2 = p3;
<a name="224"><font color="#888888 "><i>  224   </i></font></a>    p3 = itmp;
<a name="225"><font color="#888888 "><i>  225   </i></font></a>    b_x[1] = absx11;
<a name="226"><font color="#888888 "><i>  226   </i></font></a>    b_x[2] = absx21;
<a name="227"><font color="#888888 "><i>  227   </i></font></a>    absx11 = b_x[4];
<a name="228"><font color="#888888 "><i>  228   </i></font></a>    b_x[4] = b_x[5];
<a name="229"><font color="#888888 "><i>  229   </i></font></a>    b_x[5] = absx11;
<a name="230"><font color="#888888 "><i>  230   </i></font></a>    absx11 = b_x[7];
<a name="231"><font color="#888888 "><i>  231   </i></font></a>    b_x[7] = b_x[8];
<a name="232"><font color="#888888 "><i>  232   </i></font></a>    b_x[8] = absx11;
<a name="233"><font color="#888888 "><i>  233   </i></font></a>  <b>}</b>
<a name="234"><font color="#888888 "><i>  234   </i></font></a>
<a name="235"><font color="#888888 "><i>  235   </i></font></a>  absx31 = b_x[5];
<a name="236"><font color="#888888 "><i>  236   </i></font></a>  b_y = b_x[4];
<a name="237"><font color="#888888 "><i>  237   </i></font></a>  absx21 = b_x[5] / b_x[4];
<a name="238"><font color="#888888 "><i>  238   </i></font></a>  b_x[8] -= absx21 * b_x[7];
<a name="239"><font color="#888888 "><i>  239   </i></font></a>  absx11 = (absx21 * b_x[1] - b_x[2]) / b_x[8];
<a name="240"><font color="#888888 "><i>  240   </i></font></a>  absx21 = -(b_x[1] + b_x[7] * absx11) / b_x[4];
<a name="241"><font color="#888888 "><i>  241   </i></font></a>  y[p1] = ((1.0F - b_x[3] * absx21) - b_x[6] * absx11) / b_x[0];
<a name="242"><font color="#888888 "><i>  242   </i></font></a>  y[p1 + 1] = absx21;
<a name="243"><font color="#888888 "><i>  243   </i></font></a>  y[p1 + 2] = absx11;
<a name="244"><font color="#888888 "><i>  244   </i></font></a>  absx11 = -(absx31 / b_y) / b_x[8];
<a name="245"><font color="#888888 "><i>  245   </i></font></a>  absx21 = (1.0F - b_x[7] * absx11) / b_x[4];
<a name="246"><font color="#888888 "><i>  246   </i></font></a>  y[p2] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
<a name="247"><font color="#888888 "><i>  247   </i></font></a>  y[p2 + 1] = absx21;
<a name="248"><font color="#888888 "><i>  248   </i></font></a>  y[p2 + 2] = absx11;
<a name="249"><font color="#888888 "><i>  249   </i></font></a>  absx11 = 1.0F / b_x[8];
<a name="250"><font color="#888888 "><i>  250   </i></font></a>  absx21 = -b_x[7] * absx11 / b_x[4];
<a name="251"><font color="#888888 "><i>  251   </i></font></a>  y[p3] = -(b_x[3] * absx21 + b_x[6] * absx11) / b_x[0];
<a name="252"><font color="#888888 "><i>  252   </i></font></a>  y[p3 + 1] = absx21;
<a name="253"><font color="#888888 "><i>  253   </i></font></a>  y[p3 + 2] = absx11;
<a name="254"><font color="#888888 "><i>  254   </i></font></a><b>}</b>
<a name="255"><font color="#888888 "><i>  255   </i></font></a>
<a name="256"><font color="#888888 "><i>  256   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="257"><font color="#888888 "><i>  257   </i></font></a><font color="#117755"><I> * Arguments    : float A[108]</i></font>
<a name="258"><font color="#888888 "><i>  258   </i></font></a><font color="#117755"><I> *                const float B[81]</i></font>
<a name="259"><font color="#888888 "><i>  259   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="260"><font color="#888888 "><i>  260   </i></font></a><font color="#117755"><I> */</i></font>
<a name="261"><font color="#888888 "><i>  261   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>void</b></font> <a name="fcn_mrdivide">mrdivide</a>(<font color="#112266"><b>float</b></font> A[108], <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> B[81])
<a name="262"><font color="#888888 "><i>  262   </i></font></a><b>{</b>
<a name="263"><font color="#888888 "><i>  263   </i></font></a>  <font color="#112266"><b>float</b></font> b_A[81];
<a name="264"><font color="#888888 "><i>  264   </i></font></a>  <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> ipiv[9];
<a name="265"><font color="#888888 "><i>  265   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="266"><font color="#888888 "><i>  266   </i></font></a>  <font color="#112266"><b>int</b></font> j;
<a name="267"><font color="#888888 "><i>  267   </i></font></a>  <font color="#112266"><b>int</b></font> c;
<a name="268"><font color="#888888 "><i>  268   </i></font></a>  <font color="#112266"><b>int</b></font> jAcol;
<a name="269"><font color="#888888 "><i>  269   </i></font></a>  <font color="#112266"><b>int</b></font> ix;
<a name="270"><font color="#888888 "><i>  270   </i></font></a>  <font color="#112266"><b>float</b></font> temp;
<a name="271"><font color="#888888 "><i>  271   </i></font></a>  <font color="#112266"><b>float</b></font> s;
<a name="272"><font color="#888888 "><i>  272   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="273"><font color="#888888 "><i>  273   </i></font></a>  <font color="#112266"><b>int</b></font> jBcol;
<a name="274"><font color="#888888 "><i>  274   </i></font></a>  <font color="#112266"><b>int</b></font> kBcol;
<a name="275"><font color="#888888 "><i>  275   </i></font></a>  memcpy(&amp;b_A[0], &amp;B[0], 81U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>));
<a name="276"><font color="#888888 "><i>  276   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 9; k++) <b>{</b>
<a name="277"><font color="#888888 "><i>  277   </i></font></a>    ipiv[k] = (<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>)(1 + k);
<a name="278"><font color="#888888 "><i>  278   </i></font></a>  <b>}</b>
<a name="279"><font color="#888888 "><i>  279   </i></font></a>
<a name="280"><font color="#888888 "><i>  280   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 0; j &lt; 8; j++) <b>{</b>
<a name="281"><font color="#888888 "><i>  281   </i></font></a>    c = j * 10;
<a name="282"><font color="#888888 "><i>  282   </i></font></a>    jAcol = 0;
<a name="283"><font color="#888888 "><i>  283   </i></font></a>    ix = c;
<a name="284"><font color="#888888 "><i>  284   </i></font></a>    temp = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_A[c]);
<a name="285"><font color="#888888 "><i>  285   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 2; k &lt;= 9 - j; k++) <b>{</b>
<a name="286"><font color="#888888 "><i>  286   </i></font></a>      ix++;
<a name="287"><font color="#888888 "><i>  287   </i></font></a>      s = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(b_A[ix]);
<a name="288"><font color="#888888 "><i>  288   </i></font></a>      <font color="#112266"><b>if</b></font> (s &gt; temp) <b>{</b>
<a name="289"><font color="#888888 "><i>  289   </i></font></a>        jAcol = k - 1;
<a name="290"><font color="#888888 "><i>  290   </i></font></a>        temp = s;
<a name="291"><font color="#888888 "><i>  291   </i></font></a>      <b>}</b>
<a name="292"><font color="#888888 "><i>  292   </i></font></a>    <b>}</b>
<a name="293"><font color="#888888 "><i>  293   </i></font></a>
<a name="294"><font color="#888888 "><i>  294   </i></font></a>    <font color="#112266"><b>if</b></font> (b_A[c + jAcol] != 0.0F) <b>{</b>
<a name="295"><font color="#888888 "><i>  295   </i></font></a>      <font color="#112266"><b>if</b></font> (jAcol != 0) <b>{</b>
<a name="296"><font color="#888888 "><i>  296   </i></font></a>        ipiv[j] = (<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>)((j + jAcol) + 1);
<a name="297"><font color="#888888 "><i>  297   </i></font></a>        ix = j;
<a name="298"><font color="#888888 "><i>  298   </i></font></a>        jAcol += j;
<a name="299"><font color="#888888 "><i>  299   </i></font></a>        <font color="#112266"><b>for</b></font> (k = 0; k &lt; 9; k++) <b>{</b>
<a name="300"><font color="#888888 "><i>  300   </i></font></a>          temp = b_A[ix];
<a name="301"><font color="#888888 "><i>  301   </i></font></a>          b_A[ix] = b_A[jAcol];
<a name="302"><font color="#888888 "><i>  302   </i></font></a>          b_A[jAcol] = temp;
<a name="303"><font color="#888888 "><i>  303   </i></font></a>          ix += 9;
<a name="304"><font color="#888888 "><i>  304   </i></font></a>          jAcol += 9;
<a name="305"><font color="#888888 "><i>  305   </i></font></a>        <b>}</b>
<a name="306"><font color="#888888 "><i>  306   </i></font></a>      <b>}</b>
<a name="307"><font color="#888888 "><i>  307   </i></font></a>
<a name="308"><font color="#888888 "><i>  308   </i></font></a>      k = (c - j) + 9;
<a name="309"><font color="#888888 "><i>  309   </i></font></a>      <font color="#112266"><b>for</b></font> (i = c + 1; i + 1 &lt;= k; i++) <b>{</b>
<a name="310"><font color="#888888 "><i>  310   </i></font></a>        b_A[i] /= b_A[c];
<a name="311"><font color="#888888 "><i>  311   </i></font></a>      <b>}</b>
<a name="312"><font color="#888888 "><i>  312   </i></font></a>    <b>}</b>
<a name="313"><font color="#888888 "><i>  313   </i></font></a>
<a name="314"><font color="#888888 "><i>  314   </i></font></a>    jBcol = c;
<a name="315"><font color="#888888 "><i>  315   </i></font></a>    jAcol = c + 9;
<a name="316"><font color="#888888 "><i>  316   </i></font></a>    <font color="#112266"><b>for</b></font> (kBcol = 1; kBcol &lt;= 8 - j; kBcol++) <b>{</b>
<a name="317"><font color="#888888 "><i>  317   </i></font></a>      temp = b_A[jAcol];
<a name="318"><font color="#888888 "><i>  318   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[jAcol] != 0.0F) <b>{</b>
<a name="319"><font color="#888888 "><i>  319   </i></font></a>        ix = c + 1;
<a name="320"><font color="#888888 "><i>  320   </i></font></a>        k = (jBcol - j) + 18;
<a name="321"><font color="#888888 "><i>  321   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 10 + jBcol; i + 1 &lt;= k; i++) <b>{</b>
<a name="322"><font color="#888888 "><i>  322   </i></font></a>          b_A[i] += b_A[ix] * -temp;
<a name="323"><font color="#888888 "><i>  323   </i></font></a>          ix++;
<a name="324"><font color="#888888 "><i>  324   </i></font></a>        <b>}</b>
<a name="325"><font color="#888888 "><i>  325   </i></font></a>      <b>}</b>
<a name="326"><font color="#888888 "><i>  326   </i></font></a>
<a name="327"><font color="#888888 "><i>  327   </i></font></a>      jAcol += 9;
<a name="328"><font color="#888888 "><i>  328   </i></font></a>      jBcol += 9;
<a name="329"><font color="#888888 "><i>  329   </i></font></a>    <b>}</b>
<a name="330"><font color="#888888 "><i>  330   </i></font></a>  <b>}</b>
<a name="331"><font color="#888888 "><i>  331   </i></font></a>
<a name="332"><font color="#888888 "><i>  332   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 0; j &lt; 9; j++) <b>{</b>
<a name="333"><font color="#888888 "><i>  333   </i></font></a>    jBcol = 12 * j;
<a name="334"><font color="#888888 "><i>  334   </i></font></a>    jAcol = 9 * j;
<a name="335"><font color="#888888 "><i>  335   </i></font></a>    <font color="#112266"><b>for</b></font> (k = 1; k &lt;= j; k++) <b>{</b>
<a name="336"><font color="#888888 "><i>  336   </i></font></a>      kBcol = 12 * (k - 1);
<a name="337"><font color="#888888 "><i>  337   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[(k + jAcol) - 1] != 0.0F) <b>{</b>
<a name="338"><font color="#888888 "><i>  338   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="339"><font color="#888888 "><i>  339   </i></font></a>          A[i + jBcol] -= b_A[(k + jAcol) - 1] * A[i + kBcol];
<a name="340"><font color="#888888 "><i>  340   </i></font></a>        <b>}</b>
<a name="341"><font color="#888888 "><i>  341   </i></font></a>      <b>}</b>
<a name="342"><font color="#888888 "><i>  342   </i></font></a>    <b>}</b>
<a name="343"><font color="#888888 "><i>  343   </i></font></a>
<a name="344"><font color="#888888 "><i>  344   </i></font></a>    temp = 1.0F / b_A[j + jAcol];
<a name="345"><font color="#888888 "><i>  345   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="346"><font color="#888888 "><i>  346   </i></font></a>      A[i + jBcol] *= temp;
<a name="347"><font color="#888888 "><i>  347   </i></font></a>    <b>}</b>
<a name="348"><font color="#888888 "><i>  348   </i></font></a>  <b>}</b>
<a name="349"><font color="#888888 "><i>  349   </i></font></a>
<a name="350"><font color="#888888 "><i>  350   </i></font></a>  <font color="#112266"><b>for</b></font> (j = 8; j &gt; -1; j += -1) <b>{</b>
<a name="351"><font color="#888888 "><i>  351   </i></font></a>    jBcol = 12 * j;
<a name="352"><font color="#888888 "><i>  352   </i></font></a>    jAcol = 9 * j - 1;
<a name="353"><font color="#888888 "><i>  353   </i></font></a>    <font color="#112266"><b>for</b></font> (k = j + 2; k &lt; 10; k++) <b>{</b>
<a name="354"><font color="#888888 "><i>  354   </i></font></a>      kBcol = 12 * (k - 1);
<a name="355"><font color="#888888 "><i>  355   </i></font></a>      <font color="#112266"><b>if</b></font> (b_A[k + jAcol] != 0.0F) <b>{</b>
<a name="356"><font color="#888888 "><i>  356   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="357"><font color="#888888 "><i>  357   </i></font></a>          A[i + jBcol] -= b_A[k + jAcol] * A[i + kBcol];
<a name="358"><font color="#888888 "><i>  358   </i></font></a>        <b>}</b>
<a name="359"><font color="#888888 "><i>  359   </i></font></a>      <b>}</b>
<a name="360"><font color="#888888 "><i>  360   </i></font></a>    <b>}</b>
<a name="361"><font color="#888888 "><i>  361   </i></font></a>  <b>}</b>
<a name="362"><font color="#888888 "><i>  362   </i></font></a>
<a name="363"><font color="#888888 "><i>  363   </i></font></a>  <font color="#112266"><b>for</b></font> (jAcol = 7; jAcol &gt; -1; jAcol += -1) <b>{</b>
<a name="364"><font color="#888888 "><i>  364   </i></font></a>    <font color="#112266"><b>if</b></font> (ipiv[jAcol] != jAcol + 1) <b>{</b>
<a name="365"><font color="#888888 "><i>  365   </i></font></a>      <font color="#112266"><b>for</b></font> (jBcol = 0; jBcol &lt; 12; jBcol++) <b>{</b>
<a name="366"><font color="#888888 "><i>  366   </i></font></a>        temp = A[jBcol + 12 * jAcol];
<a name="367"><font color="#888888 "><i>  367   </i></font></a>        A[jBcol + 12 * jAcol] = A[jBcol + 12 * (ipiv[jAcol] - 1)];
<a name="368"><font color="#888888 "><i>  368   </i></font></a>        A[jBcol + 12 * (ipiv[jAcol] - 1)] = temp;
<a name="369"><font color="#888888 "><i>  369   </i></font></a>      <b>}</b>
<a name="370"><font color="#888888 "><i>  370   </i></font></a>    <b>}</b>
<a name="371"><font color="#888888 "><i>  371   </i></font></a>  <b>}</b>
<a name="372"><font color="#888888 "><i>  372   </i></font></a><b>}</b>
<a name="373"><font color="#888888 "><i>  373   </i></font></a>
<a name="374"><font color="#888888 "><i>  374   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="375"><font color="#888888 "><i>  375   </i></font></a><font color="#117755"><I> * Arguments    : const float x[3]</i></font>
<a name="376"><font color="#888888 "><i>  376   </i></font></a><font color="#117755"><I> * Return Type  : float</i></font>
<a name="377"><font color="#888888 "><i>  377   </i></font></a><font color="#117755"><I> */</i></font>
<a name="378"><font color="#888888 "><i>  378   </i></font></a><font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> <a name="fcn_norm">norm</a>(<font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> x[3])
<a name="379"><font color="#888888 "><i>  379   </i></font></a><b>{</b>
<a name="380"><font color="#888888 "><i>  380   </i></font></a>  <font color="#112266"><b>float</b></font> y;
<a name="381"><font color="#888888 "><i>  381   </i></font></a>  <font color="#112266"><b>float</b></font> scale;
<a name="382"><font color="#888888 "><i>  382   </i></font></a>  <font color="#112266"><b>int</b></font> k;
<a name="383"><font color="#888888 "><i>  383   </i></font></a>  <font color="#112266"><b>float</b></font> absxk;
<a name="384"><font color="#888888 "><i>  384   </i></font></a>  <font color="#112266"><b>float</b></font> t;
<a name="385"><font color="#888888 "><i>  385   </i></font></a>  y = 0.0F;
<a name="386"><font color="#888888 "><i>  386   </i></font></a>  scale = 1.17549435E-38F;
<a name="387"><font color="#888888 "><i>  387   </i></font></a>  <font color="#112266"><b>for</b></font> (k = 0; k &lt; 3; k++) <b>{</b>
<a name="388"><font color="#888888 "><i>  388   </i></font></a>    absxk = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(x[k]);
<a name="389"><font color="#888888 "><i>  389   </i></font></a>    <font color="#112266"><b>if</b></font> (absxk &gt; scale) <b>{</b>
<a name="390"><font color="#888888 "><i>  390   </i></font></a>      t = scale / absxk;
<a name="391"><font color="#888888 "><i>  391   </i></font></a>      y = 1.0F + y * t * t;
<a name="392"><font color="#888888 "><i>  392   </i></font></a>      scale = absxk;
<a name="393"><font color="#888888 "><i>  393   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="394"><font color="#888888 "><i>  394   </i></font></a>      t = absxk / scale;
<a name="395"><font color="#888888 "><i>  395   </i></font></a>      y += t * t;
<a name="396"><font color="#888888 "><i>  396   </i></font></a>    <b>}</b>
<a name="397"><font color="#888888 "><i>  397   </i></font></a>  <b>}</b>
<a name="398"><font color="#888888 "><i>  398   </i></font></a>
<a name="399"><font color="#888888 "><i>  399   </i></font></a>  <font color="#112266"><b>return</b></font> scale * (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)sqrt(y);
<a name="400"><font color="#888888 "><i>  400   </i></font></a><b>}</b>
<a name="401"><font color="#888888 "><i>  401   </i></font></a>
<a name="402"><font color="#888888 "><i>  402   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="403"><font color="#888888 "><i>  403   </i></font></a><font color="#117755"><I> * LQG Postion Estimator and Controller</i></font>
<a name="404"><font color="#888888 "><i>  404   </i></font></a><font color="#117755"><I> *  Observer:</i></font>
<a name="405"><font color="#888888 "><i>  405   </i></font></a><font color="#117755"><I> *         x[n|n]   = x[n|n-1] + M(y[n] - Cx[n|n-1] - Du[n])</i></font>
<a name="406"><font color="#888888 "><i>  406   </i></font></a><font color="#117755"><I> *         x[n+1|n] = Ax[n|n] + Bu[n]</i></font>
<a name="407"><font color="#888888 "><i>  407   </i></font></a><font color="#117755"><I> *</i></font>
<a name="408"><font color="#888888 "><i>  408   </i></font></a><font color="#117755"><I> *  $Author: Tobias Naegeli $    $Date: 2014 $    $Revision: 3 $</i></font>
<a name="409"><font color="#888888 "><i>  409   </i></font></a><font color="#117755"><I> *</i></font>
<a name="410"><font color="#888888 "><i>  410   </i></font></a><font color="#117755"><I> *</i></font>
<a name="411"><font color="#888888 "><i>  411   </i></font></a><font color="#117755"><I> *  Arguments:</i></font>
<a name="412"><font color="#888888 "><i>  412   </i></font></a><font color="#117755"><I> *  approx_prediction: if 1 then the exponential map is approximated with a</i></font>
<a name="413"><font color="#888888 "><i>  413   </i></font></a><font color="#117755"><I> *  first order taylor approximation. has at the moment not a big influence</i></font>
<a name="414"><font color="#888888 "><i>  414   </i></font></a><font color="#117755"><I> *  (just 1st or 2nd order approximation) we should change it to rodriquez</i></font>
<a name="415"><font color="#888888 "><i>  415   </i></font></a><font color="#117755"><I> *  approximation.</i></font>
<a name="416"><font color="#888888 "><i>  416   </i></font></a><font color="#117755"><I> *  use_inertia_matrix: set to true if you have the inertia matrix J for your</i></font>
<a name="417"><font color="#888888 "><i>  417   </i></font></a><font color="#117755"><I> *  quadrotor</i></font>
<a name="418"><font color="#888888 "><i>  418   </i></font></a><font color="#117755"><I> *  xa_apo_k: old state vectotr</i></font>
<a name="419"><font color="#888888 "><i>  419   </i></font></a><font color="#117755"><I> *  zFlag: if sensor measurement is available [gyro, acc, mag]</i></font>
<a name="420"><font color="#888888 "><i>  420   </i></font></a><font color="#117755"><I> *  dt: dt in s</i></font>
<a name="421"><font color="#888888 "><i>  421   </i></font></a><font color="#117755"><I> *  z: measurements [gyro, acc, mag]</i></font>
<a name="422"><font color="#888888 "><i>  422   </i></font></a><font color="#117755"><I> *  q_rotSpeed: process noise gyro</i></font>
<a name="423"><font color="#888888 "><i>  423   </i></font></a><font color="#117755"><I> *  q_rotAcc: process noise gyro acceleration</i></font>
<a name="424"><font color="#888888 "><i>  424   </i></font></a><font color="#117755"><I> *  q_acc: process noise acceleration</i></font>
<a name="425"><font color="#888888 "><i>  425   </i></font></a><font color="#117755"><I> *  q_mag: process noise magnetometer</i></font>
<a name="426"><font color="#888888 "><i>  426   </i></font></a><font color="#117755"><I> *  r_gyro: measurement noise gyro</i></font>
<a name="427"><font color="#888888 "><i>  427   </i></font></a><font color="#117755"><I> *  r_accel: measurement noise accel</i></font>
<a name="428"><font color="#888888 "><i>  428   </i></font></a><font color="#117755"><I> *  r_mag: measurement noise mag</i></font>
<a name="429"><font color="#888888 "><i>  429   </i></font></a><font color="#117755"><I> *  J: moment of inertia matrix</i></font>
<a name="430"><font color="#888888 "><i>  430   </i></font></a><font color="#117755"><I> * Arguments    : unsigned char approx_prediction</i></font>
<a name="431"><font color="#888888 "><i>  431   </i></font></a><font color="#117755"><I> *                unsigned char use_inertia_matrix</i></font>
<a name="432"><font color="#888888 "><i>  432   </i></font></a><font color="#117755"><I> *                const unsigned char zFlag[3]</i></font>
<a name="433"><font color="#888888 "><i>  433   </i></font></a><font color="#117755"><I> *                float dt</i></font>
<a name="434"><font color="#888888 "><i>  434   </i></font></a><font color="#117755"><I> *                const float z[9]</i></font>
<a name="435"><font color="#888888 "><i>  435   </i></font></a><font color="#117755"><I> *                float q_rotSpeed</i></font>
<a name="436"><font color="#888888 "><i>  436   </i></font></a><font color="#117755"><I> *                float q_rotAcc</i></font>
<a name="437"><font color="#888888 "><i>  437   </i></font></a><font color="#117755"><I> *                float q_acc</i></font>
<a name="438"><font color="#888888 "><i>  438   </i></font></a><font color="#117755"><I> *                float q_mag</i></font>
<a name="439"><font color="#888888 "><i>  439   </i></font></a><font color="#117755"><I> *                float r_gyro</i></font>
<a name="440"><font color="#888888 "><i>  440   </i></font></a><font color="#117755"><I> *                float r_accel</i></font>
<a name="441"><font color="#888888 "><i>  441   </i></font></a><font color="#117755"><I> *                float r_mag</i></font>
<a name="442"><font color="#888888 "><i>  442   </i></font></a><font color="#117755"><I> *                const float J[9]</i></font>
<a name="443"><font color="#888888 "><i>  443   </i></font></a><font color="#117755"><I> *                float xa_apo[12]</i></font>
<a name="444"><font color="#888888 "><i>  444   </i></font></a><font color="#117755"><I> *                float Pa_apo[144]</i></font>
<a name="445"><font color="#888888 "><i>  445   </i></font></a><font color="#117755"><I> *                float Rot_matrix[9]</i></font>
<a name="446"><font color="#888888 "><i>  446   </i></font></a><font color="#117755"><I> *                float eulerAngles[3]</i></font>
<a name="447"><font color="#888888 "><i>  447   </i></font></a><font color="#117755"><I> *                float debugOutput[4]</i></font>
<a name="448"><font color="#888888 "><i>  448   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="449"><font color="#888888 "><i>  449   </i></font></a><font color="#117755"><I> */</i></font>
<a name="450"><font color="#888888 "><i>  450   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_AttitudeEKF">AttitudeEKF</a>(<font color="#112266"><b>unsigned</b></font> <font color="#112266"><b>char</b></font> approx_prediction, <font color="#112266"><b>unsigned</b></font> <font color="#112266"><b>char</b></font>
<a name="451"><font color="#888888 "><i>  451   </i></font></a>                 use_inertia_matrix, <font color="#112266"><b>const</b></font> <font color="#112266"><b>unsigned</b></font> <font color="#112266"><b>char</b></font> zFlag[3], <font color="#112266"><b>float</b></font> dt,
<a name="452"><font color="#888888 "><i>  452   </i></font></a>                 <font color="#112266"><b>const</b></font> <font color="#112266"><b>float</b></font> z[9], <font color="#112266"><b>float</b></font> q_rotSpeed, <font color="#112266"><b>float</b></font> q_rotAcc, <font color="#112266"><b>float</b></font> q_acc,
<a name="453"><font color="#888888 "><i>  453   </i></font></a>                 <font color="#112266"><b>float</b></font> q_mag, <font color="#112266"><b>float</b></font> r_gyro, <font color="#112266"><b>float</b></font> r_accel, <font color="#112266"><b>float</b></font> r_mag, <font color="#112266"><b>const</b></font>
<a name="454"><font color="#888888 "><i>  454   </i></font></a>                 <font color="#112266"><b>float</b></font> J[9], <font color="#112266"><b>float</b></font> xa_apo[12], <font color="#112266"><b>float</b></font> Pa_apo[144], <font color="#112266"><b>float</b></font>
<a name="455"><font color="#888888 "><i>  455   </i></font></a>                 Rot_matrix[9], <font color="#112266"><b>float</b></font> eulerAngles[3], <font color="#112266"><b>float</b></font> debugOutput[4])
<a name="456"><font color="#888888 "><i>  456   </i></font></a><b>{</b>
<a name="457"><font color="#888888 "><i>  457   </i></font></a>  <font color="#112266"><b>int</b></font> i;
<a name="458"><font color="#888888 "><i>  458   </i></font></a>  <font color="#112266"><b>float</b></font> fv0[3];
<a name="459"><font color="#888888 "><i>  459   </i></font></a>  <font color="#112266"><b>int</b></font> r2;
<a name="460"><font color="#888888 "><i>  460   </i></font></a>  <font color="#112266"><b>float</b></font> zek[3];
<a name="461"><font color="#888888 "><i>  461   </i></font></a>  <font color="#112266"><b>float</b></font> muk[3];
<a name="462"><font color="#888888 "><i>  462   </i></font></a>  <font color="#112266"><b>float</b></font> b_muk[3];
<a name="463"><font color="#888888 "><i>  463   </i></font></a>  <font color="#112266"><b>float</b></font> c_muk[3];
<a name="464"><font color="#888888 "><i>  464   </i></font></a>  <font color="#112266"><b>float</b></font> fv1[3];
<a name="465"><font color="#888888 "><i>  465   </i></font></a>  <font color="#112266"><b>float</b></font> wak[3];
<a name="466"><font color="#888888 "><i>  466   </i></font></a>  <font color="#112266"><b>float</b></font> O[9];
<a name="467"><font color="#888888 "><i>  467   </i></font></a>  <font color="#112266"><b>float</b></font> b_O[9];
<a name="468"><font color="#888888 "><i>  468   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv0[9] = <b>{</b> 1, 0, 0, 0, 1, 0, 0, 0, 1 <b>}</b>;
<a name="469"><font color="#888888 "><i>  469   </i></font></a>
<a name="470"><font color="#888888 "><i>  470   </i></font></a>  <font color="#112266"><b>float</b></font> fv2[3];
<a name="471"><font color="#888888 "><i>  471   </i></font></a>  <font color="#112266"><b>float</b></font> maxval;
<a name="472"><font color="#888888 "><i>  472   </i></font></a>  <font color="#112266"><b>int</b></font> r1;
<a name="473"><font color="#888888 "><i>  473   </i></font></a>  <font color="#112266"><b>float</b></font> fv3[9];
<a name="474"><font color="#888888 "><i>  474   </i></font></a>  <font color="#112266"><b>float</b></font> fv4[3];
<a name="475"><font color="#888888 "><i>  475   </i></font></a>  <font color="#112266"><b>float</b></font> x_apr[12];
<a name="476"><font color="#888888 "><i>  476   </i></font></a>  <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> I[144];
<a name="477"><font color="#888888 "><i>  477   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> A_lin[144];
<a name="478"><font color="#888888 "><i>  478   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> iv1[36] = <b>{</b> 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1,
<a name="479"><font color="#888888 "><i>  479   </i></font></a>    0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 <b>}</b>;
<a name="480"><font color="#888888 "><i>  480   </i></font></a>
<a name="481"><font color="#888888 "><i>  481   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>float</b></font> b_A_lin[144];
<a name="482"><font color="#888888 "><i>  482   </i></font></a>  <font color="#112266"><b>float</b></font> v[12];
<a name="483"><font color="#888888 "><i>  483   </i></font></a>  <font color="#112266"><b>float</b></font> P_apr[144];
<a name="484"><font color="#888888 "><i>  484   </i></font></a>  <font color="#112266"><b>float</b></font> a[108];
<a name="485"><font color="#888888 "><i>  485   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> b_a[108] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0,
<a name="486"><font color="#888888 "><i>  486   </i></font></a>    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="487"><font color="#888888 "><i>  487   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="488"><font color="#888888 "><i>  488   </i></font></a>    0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0,
<a name="489"><font color="#888888 "><i>  489   </i></font></a>    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 <b>}</b>;
<a name="490"><font color="#888888 "><i>  490   </i></font></a>
<a name="491"><font color="#888888 "><i>  491   </i></font></a>  <font color="#112266"><b>float</b></font> S_k[81];
<a name="492"><font color="#888888 "><i>  492   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> b[108] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
<a name="493"><font color="#888888 "><i>  493   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="494"><font color="#888888 "><i>  494   </i></font></a>    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="495"><font color="#888888 "><i>  495   </i></font></a>    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="496"><font color="#888888 "><i>  496   </i></font></a>    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 <b>}</b>;
<a name="497"><font color="#888888 "><i>  497   </i></font></a>
<a name="498"><font color="#888888 "><i>  498   </i></font></a>  <font color="#112266"><b>float</b></font> b_r_gyro[9];
<a name="499"><font color="#888888 "><i>  499   </i></font></a>  <font color="#112266"><b>float</b></font> K_k[108];
<a name="500"><font color="#888888 "><i>  500   </i></font></a>  <font color="#112266"><b>float</b></font> b_S_k[36];
<a name="501"><font color="#888888 "><i>  501   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> c_a[36] = <b>{</b> 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0,
<a name="502"><font color="#888888 "><i>  502   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 <b>}</b>;
<a name="503"><font color="#888888 "><i>  503   </i></font></a>
<a name="504"><font color="#888888 "><i>  504   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> b_b[36] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
<a name="505"><font color="#888888 "><i>  505   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 <b>}</b>;
<a name="506"><font color="#888888 "><i>  506   </i></font></a>
<a name="507"><font color="#888888 "><i>  507   </i></font></a>  <font color="#112266"><b>float</b></font> c_r_gyro[3];
<a name="508"><font color="#888888 "><i>  508   </i></font></a>  <font color="#112266"><b>float</b></font> y[36];
<a name="509"><font color="#888888 "><i>  509   </i></font></a>  <font color="#112266"><b>int</b></font> r3;
<a name="510"><font color="#888888 "><i>  510   </i></font></a>  <font color="#112266"><b>float</b></font> a21;
<a name="511"><font color="#888888 "><i>  511   </i></font></a>  <font color="#112266"><b>float</b></font> d_a[72];
<a name="512"><font color="#888888 "><i>  512   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> e_a[72] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
<a name="513"><font color="#888888 "><i>  513   </i></font></a>    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
<a name="514"><font color="#888888 "><i>  514   </i></font></a>    0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="515"><font color="#888888 "><i>  515   </i></font></a>    0, 0, 0, 0, 0, 0 <b>}</b>;
<a name="516"><font color="#888888 "><i>  516   </i></font></a>
<a name="517"><font color="#888888 "><i>  517   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> c_b[72] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
<a name="518"><font color="#888888 "><i>  518   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="519"><font color="#888888 "><i>  519   </i></font></a>    0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="520"><font color="#888888 "><i>  520   </i></font></a>    0, 0, 1, 0, 0, 0 <b>}</b>;
<a name="521"><font color="#888888 "><i>  521   </i></font></a>
<a name="522"><font color="#888888 "><i>  522   </i></font></a>  <font color="#112266"><b>float</b></font> d_r_gyro[6];
<a name="523"><font color="#888888 "><i>  523   </i></font></a>  <font color="#112266"><b>float</b></font> c_S_k[6];
<a name="524"><font color="#888888 "><i>  524   </i></font></a>  <font color="#112266"><b>float</b></font> b_K_k[72];
<a name="525"><font color="#888888 "><i>  525   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> f_a[72] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0,
<a name="526"><font color="#888888 "><i>  526   </i></font></a>    1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="527"><font color="#888888 "><i>  527   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0,
<a name="528"><font color="#888888 "><i>  528   </i></font></a>    0, 0, 0, 0, 0, 1 <b>}</b>;
<a name="529"><font color="#888888 "><i>  529   </i></font></a>
<a name="530"><font color="#888888 "><i>  530   </i></font></a>  <font color="#112266"><b>static</b></font> <font color="#112266"><b>const</b></font> <font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font> d_b[72] = <b>{</b> 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1,
<a name="531"><font color="#888888 "><i>  531   </i></font></a>    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
<a name="532"><font color="#888888 "><i>  532   </i></font></a>    0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0,
<a name="533"><font color="#888888 "><i>  533   </i></font></a>    0, 0, 0, 0, 0, 1 <b>}</b>;
<a name="534"><font color="#888888 "><i>  534   </i></font></a>
<a name="535"><font color="#888888 "><i>  535   </i></font></a>  <font color="#112266"><b>float</b></font> b_z[6];
<a name="536"><font color="#888888 "><i>  536   </i></font></a>
<a name="537"><font color="#888888 "><i>  537   </i></font></a>  <font color="#117755"><i>/*  Output: */</i></font>
<a name="538"><font color="#888888 "><i>  538   </i></font></a>  <font color="#117755"><i>/*  xa_apo: updated state vectotr */</i></font>
<a name="539"><font color="#888888 "><i>  539   </i></font></a>  <font color="#117755"><i>/*  Pa_apo: updated state covariance matrix */</i></font>
<a name="540"><font color="#888888 "><i>  540   </i></font></a>  <font color="#117755"><i>/*  Rot_matrix: rotation matrix */</i></font>
<a name="541"><font color="#888888 "><i>  541   </i></font></a>  <font color="#117755"><i>/*  eulerAngles: euler angles */</i></font>
<a name="542"><font color="#888888 "><i>  542   </i></font></a>  <font color="#117755"><i>/*  debugOutput: not used */</i></font>
<a name="543"><font color="#888888 "><i>  543   </i></font></a>  <font color="#117755"><i>/* % model specific parameters */</i></font>
<a name="544"><font color="#888888 "><i>  544   </i></font></a>  <font color="#117755"><i>/*  compute once the inverse of the Inertia */</i></font>
<a name="545"><font color="#888888 "><i>  545   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,48">'AttitudeEKF:48'</a></span> if isempty(Ji) */</i></font>
<a name="546"><font color="#888888 "><i>  546   </i></font></a>  <font color="#112266"><b>if</b></font> (!<a href="#var_Ji_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Ji_not_empty');" target="_self"><font color="#1122aa">Ji_not_empty</font></a>) <b>{</b>
<a name="547"><font color="#888888 "><i>  547   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,49">'AttitudeEKF:49'</a></span> Ji=single(inv(J)); */</i></font>
<a name="548"><font color="#888888 "><i>  548   </i></font></a>    <a href="#fcn_inv" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_inv');" target="_self"><font color="#1122aa">inv</font></a>(J, <a href="#var_Ji" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Ji');" target="_self"><font color="#1122aa">Ji</font></a>);
<a name="549"><font color="#888888 "><i>  549   </i></font></a>    <a href="#var_Ji_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Ji_not_empty');" target="_self"><font color="#1122aa">Ji_not_empty</font></a> = true;
<a name="550"><font color="#888888 "><i>  550   </i></font></a>  <b>}</b>
<a name="551"><font color="#888888 "><i>  551   </i></font></a>
<a name="552"><font color="#888888 "><i>  552   </i></font></a>  <font color="#117755"><i>/* % init */</i></font>
<a name="553"><font color="#888888 "><i>  553   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,54">'AttitudeEKF:54'</a></span> if(isempty(x_apo)) */</i></font>
<a name="554"><font color="#888888 "><i>  554   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,64">'AttitudeEKF:64'</a></span> if(isempty(P_apo)) */</i></font>
<a name="555"><font color="#888888 "><i>  555   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,69">'AttitudeEKF:69'</a></span> debugOutput = single(zeros(4,1)); */</i></font>
<a name="556"><font color="#888888 "><i>  556   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 4; i++) <b>{</b>
<a name="557"><font color="#888888 "><i>  557   </i></font></a>    debugOutput[i] = 0.0F;
<a name="558"><font color="#888888 "><i>  558   </i></font></a>  <b>}</b>
<a name="559"><font color="#888888 "><i>  559   </i></font></a>
<a name="560"><font color="#888888 "><i>  560   </i></font></a>  <font color="#117755"><i>/* % copy the states */</i></font>
<a name="561"><font color="#888888 "><i>  561   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,72">'AttitudeEKF:72'</a></span> wx=  x_apo(1); */</i></font>
<a name="562"><font color="#888888 "><i>  562   </i></font></a>  <font color="#117755"><i>/*  x  body angular rate */</i></font>
<a name="563"><font color="#888888 "><i>  563   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,73">'AttitudeEKF:73'</a></span> wy=  x_apo(2); */</i></font>
<a name="564"><font color="#888888 "><i>  564   </i></font></a>  <font color="#117755"><i>/*  y  body angular rate */</i></font>
<a name="565"><font color="#888888 "><i>  565   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,74">'AttitudeEKF:74'</a></span> wz=  x_apo(3); */</i></font>
<a name="566"><font color="#888888 "><i>  566   </i></font></a>  <font color="#117755"><i>/*  z  body angular rate */</i></font>
<a name="567"><font color="#888888 "><i>  567   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,76">'AttitudeEKF:76'</a></span> wax=  x_apo(4); */</i></font>
<a name="568"><font color="#888888 "><i>  568   </i></font></a>  <font color="#117755"><i>/*  x  body angular acceleration */</i></font>
<a name="569"><font color="#888888 "><i>  569   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,77">'AttitudeEKF:77'</a></span> way=  x_apo(5); */</i></font>
<a name="570"><font color="#888888 "><i>  570   </i></font></a>  <font color="#117755"><i>/*  y  body angular acceleration */</i></font>
<a name="571"><font color="#888888 "><i>  571   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,78">'AttitudeEKF:78'</a></span> waz=  x_apo(6); */</i></font>
<a name="572"><font color="#888888 "><i>  572   </i></font></a>  <font color="#117755"><i>/*  z  body angular acceleration */</i></font>
<a name="573"><font color="#888888 "><i>  573   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,80">'AttitudeEKF:80'</a></span> zex=  x_apo(7); */</i></font>
<a name="574"><font color="#888888 "><i>  574   </i></font></a>  <font color="#117755"><i>/*  x  component gravity vector */</i></font>
<a name="575"><font color="#888888 "><i>  575   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,81">'AttitudeEKF:81'</a></span> zey=  x_apo(8); */</i></font>
<a name="576"><font color="#888888 "><i>  576   </i></font></a>  <font color="#117755"><i>/*  y  component gravity vector */</i></font>
<a name="577"><font color="#888888 "><i>  577   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,82">'AttitudeEKF:82'</a></span> zez=  x_apo(9); */</i></font>
<a name="578"><font color="#888888 "><i>  578   </i></font></a>  <font color="#117755"><i>/*  z  component gravity vector */</i></font>
<a name="579"><font color="#888888 "><i>  579   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,84">'AttitudeEKF:84'</a></span> mux=  x_apo(10); */</i></font>
<a name="580"><font color="#888888 "><i>  580   </i></font></a>  <font color="#117755"><i>/*  x  component magnetic field vector */</i></font>
<a name="581"><font color="#888888 "><i>  581   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,85">'AttitudeEKF:85'</a></span> muy=  x_apo(11); */</i></font>
<a name="582"><font color="#888888 "><i>  582   </i></font></a>  <font color="#117755"><i>/*  y  component magnetic field vector */</i></font>
<a name="583"><font color="#888888 "><i>  583   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,86">'AttitudeEKF:86'</a></span> muz=  x_apo(12); */</i></font>
<a name="584"><font color="#888888 "><i>  584   </i></font></a>  <font color="#117755"><i>/*  z  component magnetic field vector */</i></font>
<a name="585"><font color="#888888 "><i>  585   </i></font></a>  <font color="#117755"><i>/* % prediction section */</i></font>
<a name="586"><font color="#888888 "><i>  586   </i></font></a>  <font color="#117755"><i>/*  compute the apriori state estimate from the previous aposteriori estimate */</i></font>
<a name="587"><font color="#888888 "><i>  587   </i></font></a>  <font color="#117755"><i>/* body angular accelerations */</i></font>
<a name="588"><font color="#888888 "><i>  588   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,94">'AttitudeEKF:94'</a></span> if (use_inertia_matrix==1) */</i></font>
<a name="589"><font color="#888888 "><i>  589   </i></font></a>  <font color="#112266"><b>if</b></font> (use_inertia_matrix == 1) <b>{</b>
<a name="590"><font color="#888888 "><i>  590   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,95">'AttitudeEKF:95'</a></span> wak =[wax;way;waz]+Ji*(-cross([wax;way;waz],J*[wax;way;waz]))*dt; */</i></font>
<a name="591"><font color="#888888 "><i>  591   </i></font></a>    fv0[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3];
<a name="592"><font color="#888888 "><i>  592   </i></font></a>    fv0[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4];
<a name="593"><font color="#888888 "><i>  593   </i></font></a>    fv0[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5];
<a name="594"><font color="#888888 "><i>  594   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="595"><font color="#888888 "><i>  595   </i></font></a>      zek[r2] = 0.0F;
<a name="596"><font color="#888888 "><i>  596   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="597"><font color="#888888 "><i>  597   </i></font></a>        zek[r2] += J[r2 + 3 * i] * fv0[i];
<a name="598"><font color="#888888 "><i>  598   </i></font></a>      <b>}</b>
<a name="599"><font color="#888888 "><i>  599   </i></font></a>    <b>}</b>
<a name="600"><font color="#888888 "><i>  600   </i></font></a>
<a name="601"><font color="#888888 "><i>  601   </i></font></a>    muk[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3];
<a name="602"><font color="#888888 "><i>  602   </i></font></a>    muk[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4];
<a name="603"><font color="#888888 "><i>  603   </i></font></a>    muk[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5];
<a name="604"><font color="#888888 "><i>  604   </i></font></a>    b_muk[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4] * zek[2] - <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5] * zek[1];
<a name="605"><font color="#888888 "><i>  605   </i></font></a>    b_muk[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5] * zek[0] - <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3] * zek[2];
<a name="606"><font color="#888888 "><i>  606   </i></font></a>    b_muk[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3] * zek[1] - <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4] * zek[0];
<a name="607"><font color="#888888 "><i>  607   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="608"><font color="#888888 "><i>  608   </i></font></a>      c_muk[r2] = -b_muk[r2];
<a name="609"><font color="#888888 "><i>  609   </i></font></a>    <b>}</b>
<a name="610"><font color="#888888 "><i>  610   </i></font></a>
<a name="611"><font color="#888888 "><i>  611   </i></font></a>    fv1[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3];
<a name="612"><font color="#888888 "><i>  612   </i></font></a>    fv1[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4];
<a name="613"><font color="#888888 "><i>  613   </i></font></a>    fv1[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5];
<a name="614"><font color="#888888 "><i>  614   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="615"><font color="#888888 "><i>  615   </i></font></a>      fv0[r2] = 0.0F;
<a name="616"><font color="#888888 "><i>  616   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="617"><font color="#888888 "><i>  617   </i></font></a>        fv0[r2] += <a href="#var_Ji" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Ji');" target="_self"><font color="#1122aa">Ji</font></a>[r2 + 3 * i] * c_muk[i];
<a name="618"><font color="#888888 "><i>  618   </i></font></a>      <b>}</b>
<a name="619"><font color="#888888 "><i>  619   </i></font></a>
<a name="620"><font color="#888888 "><i>  620   </i></font></a>      wak[r2] = fv1[r2] + fv0[r2] * dt;
<a name="621"><font color="#888888 "><i>  621   </i></font></a>    <b>}</b>
<a name="622"><font color="#888888 "><i>  622   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="623"><font color="#888888 "><i>  623   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,96">'AttitudeEKF:96'</a></span> else */</i></font>
<a name="624"><font color="#888888 "><i>  624   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,97">'AttitudeEKF:97'</a></span> wak =[wax;way;waz]; */</i></font>
<a name="625"><font color="#888888 "><i>  625   </i></font></a>    wak[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[3];
<a name="626"><font color="#888888 "><i>  626   </i></font></a>    wak[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[4];
<a name="627"><font color="#888888 "><i>  627   </i></font></a>    wak[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[5];
<a name="628"><font color="#888888 "><i>  628   </i></font></a>  <b>}</b>
<a name="629"><font color="#888888 "><i>  629   </i></font></a>
<a name="630"><font color="#888888 "><i>  630   </i></font></a>  <font color="#117755"><i>/* body angular rates */</i></font>
<a name="631"><font color="#888888 "><i>  631   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,101">'AttitudeEKF:101'</a></span> wk =[wx;  wy; wz] + dt*wak; */</i></font>
<a name="632"><font color="#888888 "><i>  632   </i></font></a>  <font color="#117755"><i>/* derivative of the prediction rotation matrix */</i></font>
<a name="633"><font color="#888888 "><i>  633   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,104">'AttitudeEKF:104'</a></span> O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */</i></font>
<a name="634"><font color="#888888 "><i>  634   </i></font></a>  O[0] = 0.0F;
<a name="635"><font color="#888888 "><i>  635   </i></font></a>  O[1] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[2];
<a name="636"><font color="#888888 "><i>  636   </i></font></a>  O[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[1];
<a name="637"><font color="#888888 "><i>  637   </i></font></a>  O[3] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[2];
<a name="638"><font color="#888888 "><i>  638   </i></font></a>  O[4] = 0.0F;
<a name="639"><font color="#888888 "><i>  639   </i></font></a>  O[5] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[0];
<a name="640"><font color="#888888 "><i>  640   </i></font></a>  O[6] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[1];
<a name="641"><font color="#888888 "><i>  641   </i></font></a>  O[7] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[0];
<a name="642"><font color="#888888 "><i>  642   </i></font></a>  O[8] = 0.0F;
<a name="643"><font color="#888888 "><i>  643   </i></font></a>
<a name="644"><font color="#888888 "><i>  644   </i></font></a>  <font color="#117755"><i>/* prediction of the earth z vector */</i></font>
<a name="645"><font color="#888888 "><i>  645   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,107">'AttitudeEKF:107'</a></span> if (approx_prediction==1) */</i></font>
<a name="646"><font color="#888888 "><i>  646   </i></font></a>  <font color="#112266"><b>if</b></font> (approx_prediction == 1) <b>{</b>
<a name="647"><font color="#888888 "><i>  647   </i></font></a>    <font color="#117755"><i>/* e^(Odt)=I+dt*O+dt^2/2!O^2 */</i></font>
<a name="648"><font color="#888888 "><i>  648   </i></font></a>    <font color="#117755"><i>/*  so we do a first order approximation of the exponential map */</i></font>
<a name="649"><font color="#888888 "><i>  649   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,110">'AttitudeEKF:110'</a></span> zek =(O*dt+single(eye(3)))*[zex;zey;zez]; */</i></font>
<a name="650"><font color="#888888 "><i>  650   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="651"><font color="#888888 "><i>  651   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="652"><font color="#888888 "><i>  652   </i></font></a>        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (<font color="#112266"><b>float</b></font>)iv0[i + 3 * r2];
<a name="653"><font color="#888888 "><i>  653   </i></font></a>      <b>}</b>
<a name="654"><font color="#888888 "><i>  654   </i></font></a>    <b>}</b>
<a name="655"><font color="#888888 "><i>  655   </i></font></a>
<a name="656"><font color="#888888 "><i>  656   </i></font></a>    fv2[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[6];
<a name="657"><font color="#888888 "><i>  657   </i></font></a>    fv2[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[7];
<a name="658"><font color="#888888 "><i>  658   </i></font></a>    fv2[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[8];
<a name="659"><font color="#888888 "><i>  659   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="660"><font color="#888888 "><i>  660   </i></font></a>      zek[r2] = 0.0F;
<a name="661"><font color="#888888 "><i>  661   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="662"><font color="#888888 "><i>  662   </i></font></a>        zek[r2] += b_O[r2 + 3 * i] * fv2[i];
<a name="663"><font color="#888888 "><i>  663   </i></font></a>      <b>}</b>
<a name="664"><font color="#888888 "><i>  664   </i></font></a>    <b>}</b>
<a name="665"><font color="#888888 "><i>  665   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="666"><font color="#888888 "><i>  666   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,112">'AttitudeEKF:112'</a></span> else */</i></font>
<a name="667"><font color="#888888 "><i>  667   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,113">'AttitudeEKF:113'</a></span> zek =(single(eye(3))+O*dt+dt^2/2*O^2)*[zex;zey;zez]; */</i></font>
<a name="668"><font color="#888888 "><i>  668   </i></font></a>    maxval = dt * dt / 2.0F;
<a name="669"><font color="#888888 "><i>  669   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="670"><font color="#888888 "><i>  670   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="671"><font color="#888888 "><i>  671   </i></font></a>        b_O[r2 + 3 * i] = 0.0F;
<a name="672"><font color="#888888 "><i>  672   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 3; r1++) <b>{</b>
<a name="673"><font color="#888888 "><i>  673   </i></font></a>          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
<a name="674"><font color="#888888 "><i>  674   </i></font></a>        <b>}</b>
<a name="675"><font color="#888888 "><i>  675   </i></font></a>      <b>}</b>
<a name="676"><font color="#888888 "><i>  676   </i></font></a>    <b>}</b>
<a name="677"><font color="#888888 "><i>  677   </i></font></a>
<a name="678"><font color="#888888 "><i>  678   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="679"><font color="#888888 "><i>  679   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="680"><font color="#888888 "><i>  680   </i></font></a>        fv3[i + 3 * r2] = ((<font color="#112266"><b>float</b></font>)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
<a name="681"><font color="#888888 "><i>  681   </i></font></a>          * b_O[i + 3 * r2];
<a name="682"><font color="#888888 "><i>  682   </i></font></a>      <b>}</b>
<a name="683"><font color="#888888 "><i>  683   </i></font></a>    <b>}</b>
<a name="684"><font color="#888888 "><i>  684   </i></font></a>
<a name="685"><font color="#888888 "><i>  685   </i></font></a>    fv2[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[6];
<a name="686"><font color="#888888 "><i>  686   </i></font></a>    fv2[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[7];
<a name="687"><font color="#888888 "><i>  687   </i></font></a>    fv2[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[8];
<a name="688"><font color="#888888 "><i>  688   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="689"><font color="#888888 "><i>  689   </i></font></a>      zek[r2] = 0.0F;
<a name="690"><font color="#888888 "><i>  690   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="691"><font color="#888888 "><i>  691   </i></font></a>        zek[r2] += fv3[r2 + 3 * i] * fv2[i];
<a name="692"><font color="#888888 "><i>  692   </i></font></a>      <b>}</b>
<a name="693"><font color="#888888 "><i>  693   </i></font></a>    <b>}</b>
<a name="694"><font color="#888888 "><i>  694   </i></font></a>
<a name="695"><font color="#888888 "><i>  695   </i></font></a>    <font color="#117755"><i>/* zek =expm2(O*dt)*[zex;zey;zez]; not working because use double */</i></font>
<a name="696"><font color="#888888 "><i>  696   </i></font></a>    <font color="#117755"><i>/* precision */</i></font>
<a name="697"><font color="#888888 "><i>  697   </i></font></a>  <b>}</b>
<a name="698"><font color="#888888 "><i>  698   </i></font></a>
<a name="699"><font color="#888888 "><i>  699   </i></font></a>  <font color="#117755"><i>/* prediction of the magnetic vector */</i></font>
<a name="700"><font color="#888888 "><i>  700   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,121">'AttitudeEKF:121'</a></span> if (approx_prediction==1) */</i></font>
<a name="701"><font color="#888888 "><i>  701   </i></font></a>  <font color="#112266"><b>if</b></font> (approx_prediction == 1) <b>{</b>
<a name="702"><font color="#888888 "><i>  702   </i></font></a>    <font color="#117755"><i>/* e^(Odt)=I+dt*O+dt^2/2!O^2 */</i></font>
<a name="703"><font color="#888888 "><i>  703   </i></font></a>    <font color="#117755"><i>/*  so we do a first order approximation of the exponential map */</i></font>
<a name="704"><font color="#888888 "><i>  704   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,124">'AttitudeEKF:124'</a></span> muk =(O*dt+single(eye(3)))*[mux;muy;muz]; */</i></font>
<a name="705"><font color="#888888 "><i>  705   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="706"><font color="#888888 "><i>  706   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="707"><font color="#888888 "><i>  707   </i></font></a>        b_O[i + 3 * r2] = O[i + 3 * r2] * dt + (<font color="#112266"><b>float</b></font>)iv0[i + 3 * r2];
<a name="708"><font color="#888888 "><i>  708   </i></font></a>      <b>}</b>
<a name="709"><font color="#888888 "><i>  709   </i></font></a>    <b>}</b>
<a name="710"><font color="#888888 "><i>  710   </i></font></a>
<a name="711"><font color="#888888 "><i>  711   </i></font></a>    fv4[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[9];
<a name="712"><font color="#888888 "><i>  712   </i></font></a>    fv4[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[10];
<a name="713"><font color="#888888 "><i>  713   </i></font></a>    fv4[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[11];
<a name="714"><font color="#888888 "><i>  714   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="715"><font color="#888888 "><i>  715   </i></font></a>      muk[r2] = 0.0F;
<a name="716"><font color="#888888 "><i>  716   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="717"><font color="#888888 "><i>  717   </i></font></a>        muk[r2] += b_O[r2 + 3 * i] * fv4[i];
<a name="718"><font color="#888888 "><i>  718   </i></font></a>      <b>}</b>
<a name="719"><font color="#888888 "><i>  719   </i></font></a>    <b>}</b>
<a name="720"><font color="#888888 "><i>  720   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="721"><font color="#888888 "><i>  721   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,125">'AttitudeEKF:125'</a></span> else */</i></font>
<a name="722"><font color="#888888 "><i>  722   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,126">'AttitudeEKF:126'</a></span> muk =(single(eye(3))+O*dt+dt^2/2*O^2)*[mux;muy;muz]; */</i></font>
<a name="723"><font color="#888888 "><i>  723   </i></font></a>    maxval = dt * dt / 2.0F;
<a name="724"><font color="#888888 "><i>  724   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="725"><font color="#888888 "><i>  725   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="726"><font color="#888888 "><i>  726   </i></font></a>        b_O[r2 + 3 * i] = 0.0F;
<a name="727"><font color="#888888 "><i>  727   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 3; r1++) <b>{</b>
<a name="728"><font color="#888888 "><i>  728   </i></font></a>          b_O[r2 + 3 * i] += O[r2 + 3 * r1] * O[r1 + 3 * i];
<a name="729"><font color="#888888 "><i>  729   </i></font></a>        <b>}</b>
<a name="730"><font color="#888888 "><i>  730   </i></font></a>      <b>}</b>
<a name="731"><font color="#888888 "><i>  731   </i></font></a>    <b>}</b>
<a name="732"><font color="#888888 "><i>  732   </i></font></a>
<a name="733"><font color="#888888 "><i>  733   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="734"><font color="#888888 "><i>  734   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="735"><font color="#888888 "><i>  735   </i></font></a>        fv3[i + 3 * r2] = ((<font color="#112266"><b>float</b></font>)iv0[i + 3 * r2] + O[i + 3 * r2] * dt) + maxval
<a name="736"><font color="#888888 "><i>  736   </i></font></a>          * b_O[i + 3 * r2];
<a name="737"><font color="#888888 "><i>  737   </i></font></a>      <b>}</b>
<a name="738"><font color="#888888 "><i>  738   </i></font></a>    <b>}</b>
<a name="739"><font color="#888888 "><i>  739   </i></font></a>
<a name="740"><font color="#888888 "><i>  740   </i></font></a>    fv4[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[9];
<a name="741"><font color="#888888 "><i>  741   </i></font></a>    fv4[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[10];
<a name="742"><font color="#888888 "><i>  742   </i></font></a>    fv4[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[11];
<a name="743"><font color="#888888 "><i>  743   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="744"><font color="#888888 "><i>  744   </i></font></a>      muk[r2] = 0.0F;
<a name="745"><font color="#888888 "><i>  745   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="746"><font color="#888888 "><i>  746   </i></font></a>        muk[r2] += fv3[r2 + 3 * i] * fv4[i];
<a name="747"><font color="#888888 "><i>  747   </i></font></a>      <b>}</b>
<a name="748"><font color="#888888 "><i>  748   </i></font></a>    <b>}</b>
<a name="749"><font color="#888888 "><i>  749   </i></font></a>
<a name="750"><font color="#888888 "><i>  750   </i></font></a>    <font color="#117755"><i>/* muk =expm2(O*dt)*[mux;muy;muz]; not working because use double */</i></font>
<a name="751"><font color="#888888 "><i>  751   </i></font></a>    <font color="#117755"><i>/* precision */</i></font>
<a name="752"><font color="#888888 "><i>  752   </i></font></a>  <b>}</b>
<a name="753"><font color="#888888 "><i>  753   </i></font></a>
<a name="754"><font color="#888888 "><i>  754   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,131">'AttitudeEKF:131'</a></span> x_apr=[wk;wak;zek;muk]; */</i></font>
<a name="755"><font color="#888888 "><i>  755   </i></font></a>  x_apr[0] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[0] + dt * wak[0];
<a name="756"><font color="#888888 "><i>  756   </i></font></a>  x_apr[1] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[1] + dt * wak[1];
<a name="757"><font color="#888888 "><i>  757   </i></font></a>  x_apr[2] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[2] + dt * wak[2];
<a name="758"><font color="#888888 "><i>  758   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="759"><font color="#888888 "><i>  759   </i></font></a>    x_apr[i + 3] = wak[i];
<a name="760"><font color="#888888 "><i>  760   </i></font></a>  <b>}</b>
<a name="761"><font color="#888888 "><i>  761   </i></font></a>
<a name="762"><font color="#888888 "><i>  762   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="763"><font color="#888888 "><i>  763   </i></font></a>    x_apr[i + 6] = zek[i];
<a name="764"><font color="#888888 "><i>  764   </i></font></a>  <b>}</b>
<a name="765"><font color="#888888 "><i>  765   </i></font></a>
<a name="766"><font color="#888888 "><i>  766   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="767"><font color="#888888 "><i>  767   </i></font></a>    x_apr[i + 9] = muk[i];
<a name="768"><font color="#888888 "><i>  768   </i></font></a>  <b>}</b>
<a name="769"><font color="#888888 "><i>  769   </i></font></a>
<a name="770"><font color="#888888 "><i>  770   </i></font></a>  <font color="#117755"><i>/*  compute the apriori error covariance estimate from the previous */</i></font>
<a name="771"><font color="#888888 "><i>  771   </i></font></a>  <font color="#117755"><i>/* aposteriori estimate */</i></font>
<a name="772"><font color="#888888 "><i>  772   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,136">'AttitudeEKF:136'</a></span> EZ=[0,zez,-zey; */</i></font>
<a name="773"><font color="#888888 "><i>  773   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,137">'AttitudeEKF:137'</a></span>     -zez,0,zex; */</i></font>
<a name="774"><font color="#888888 "><i>  774   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,138">'AttitudeEKF:138'</a></span>     zey,-zex,0]'; */</i></font>
<a name="775"><font color="#888888 "><i>  775   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,139">'AttitudeEKF:139'</a></span> MA=[0,muz,-muy; */</i></font>
<a name="776"><font color="#888888 "><i>  776   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,140">'AttitudeEKF:140'</a></span>     -muz,0,mux; */</i></font>
<a name="777"><font color="#888888 "><i>  777   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,141">'AttitudeEKF:141'</a></span>     muy,-mux,0]'; */</i></font>
<a name="778"><font color="#888888 "><i>  778   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,143">'AttitudeEKF:143'</a></span> E=single(eye(3)); */</i></font>
<a name="779"><font color="#888888 "><i>  779   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,144">'AttitudeEKF:144'</a></span> Z=single(zeros(3)); */</i></font>
<a name="780"><font color="#888888 "><i>  780   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,146">'AttitudeEKF:146'</a></span> A_lin=[ Z,  E,  Z,  Z */</i></font>
<a name="781"><font color="#888888 "><i>  781   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,147">'AttitudeEKF:147'</a></span>     Z,  Z,  Z,  Z */</i></font>
<a name="782"><font color="#888888 "><i>  782   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,148">'AttitudeEKF:148'</a></span>     EZ, Z,  O,  Z */</i></font>
<a name="783"><font color="#888888 "><i>  783   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,149">'AttitudeEKF:149'</a></span>     MA, Z,  Z,  O]; */</i></font>
<a name="784"><font color="#888888 "><i>  784   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,151">'AttitudeEKF:151'</a></span> A_lin=eye(12)+A_lin*dt; */</i></font>
<a name="785"><font color="#888888 "><i>  785   </i></font></a>  memset(&amp;I[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>));
<a name="786"><font color="#888888 "><i>  786   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="787"><font color="#888888 "><i>  787   </i></font></a>    I[i + 12 * i] = 1;
<a name="788"><font color="#888888 "><i>  788   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="789"><font color="#888888 "><i>  789   </i></font></a>      A_lin[r2 + 12 * i] = iv1[r2 + 3 * i];
<a name="790"><font color="#888888 "><i>  790   </i></font></a>    <b>}</b>
<a name="791"><font color="#888888 "><i>  791   </i></font></a>
<a name="792"><font color="#888888 "><i>  792   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="793"><font color="#888888 "><i>  793   </i></font></a>      A_lin[(r2 + 12 * i) + 3] = 0.0F;
<a name="794"><font color="#888888 "><i>  794   </i></font></a>    <b>}</b>
<a name="795"><font color="#888888 "><i>  795   </i></font></a>  <b>}</b>
<a name="796"><font color="#888888 "><i>  796   </i></font></a>
<a name="797"><font color="#888888 "><i>  797   </i></font></a>  A_lin[6] = 0.0F;
<a name="798"><font color="#888888 "><i>  798   </i></font></a>  A_lin[7] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[8];
<a name="799"><font color="#888888 "><i>  799   </i></font></a>  A_lin[8] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[7];
<a name="800"><font color="#888888 "><i>  800   </i></font></a>  A_lin[18] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[8];
<a name="801"><font color="#888888 "><i>  801   </i></font></a>  A_lin[19] = 0.0F;
<a name="802"><font color="#888888 "><i>  802   </i></font></a>  A_lin[20] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[6];
<a name="803"><font color="#888888 "><i>  803   </i></font></a>  A_lin[30] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[7];
<a name="804"><font color="#888888 "><i>  804   </i></font></a>  A_lin[31] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[6];
<a name="805"><font color="#888888 "><i>  805   </i></font></a>  A_lin[32] = 0.0F;
<a name="806"><font color="#888888 "><i>  806   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="807"><font color="#888888 "><i>  807   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="808"><font color="#888888 "><i>  808   </i></font></a>      A_lin[(i + 12 * (r2 + 3)) + 6] = 0.0F;
<a name="809"><font color="#888888 "><i>  809   </i></font></a>    <b>}</b>
<a name="810"><font color="#888888 "><i>  810   </i></font></a>  <b>}</b>
<a name="811"><font color="#888888 "><i>  811   </i></font></a>
<a name="812"><font color="#888888 "><i>  812   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="813"><font color="#888888 "><i>  813   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="814"><font color="#888888 "><i>  814   </i></font></a>      A_lin[(i + 12 * (r2 + 6)) + 6] = O[i + 3 * r2];
<a name="815"><font color="#888888 "><i>  815   </i></font></a>    <b>}</b>
<a name="816"><font color="#888888 "><i>  816   </i></font></a>  <b>}</b>
<a name="817"><font color="#888888 "><i>  817   </i></font></a>
<a name="818"><font color="#888888 "><i>  818   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="819"><font color="#888888 "><i>  819   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="820"><font color="#888888 "><i>  820   </i></font></a>      A_lin[(i + 12 * (r2 + 9)) + 6] = 0.0F;
<a name="821"><font color="#888888 "><i>  821   </i></font></a>    <b>}</b>
<a name="822"><font color="#888888 "><i>  822   </i></font></a>  <b>}</b>
<a name="823"><font color="#888888 "><i>  823   </i></font></a>
<a name="824"><font color="#888888 "><i>  824   </i></font></a>  A_lin[9] = 0.0F;
<a name="825"><font color="#888888 "><i>  825   </i></font></a>  A_lin[10] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[11];
<a name="826"><font color="#888888 "><i>  826   </i></font></a>  A_lin[11] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[10];
<a name="827"><font color="#888888 "><i>  827   </i></font></a>  A_lin[21] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[11];
<a name="828"><font color="#888888 "><i>  828   </i></font></a>  A_lin[22] = 0.0F;
<a name="829"><font color="#888888 "><i>  829   </i></font></a>  A_lin[23] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[9];
<a name="830"><font color="#888888 "><i>  830   </i></font></a>  A_lin[33] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[10];
<a name="831"><font color="#888888 "><i>  831   </i></font></a>  A_lin[34] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[9];
<a name="832"><font color="#888888 "><i>  832   </i></font></a>  A_lin[35] = 0.0F;
<a name="833"><font color="#888888 "><i>  833   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="834"><font color="#888888 "><i>  834   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="835"><font color="#888888 "><i>  835   </i></font></a>      A_lin[(i + 12 * (r2 + 3)) + 9] = 0.0F;
<a name="836"><font color="#888888 "><i>  836   </i></font></a>    <b>}</b>
<a name="837"><font color="#888888 "><i>  837   </i></font></a>  <b>}</b>
<a name="838"><font color="#888888 "><i>  838   </i></font></a>
<a name="839"><font color="#888888 "><i>  839   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="840"><font color="#888888 "><i>  840   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="841"><font color="#888888 "><i>  841   </i></font></a>      A_lin[(i + 12 * (r2 + 6)) + 9] = 0.0F;
<a name="842"><font color="#888888 "><i>  842   </i></font></a>    <b>}</b>
<a name="843"><font color="#888888 "><i>  843   </i></font></a>  <b>}</b>
<a name="844"><font color="#888888 "><i>  844   </i></font></a>
<a name="845"><font color="#888888 "><i>  845   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="846"><font color="#888888 "><i>  846   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="847"><font color="#888888 "><i>  847   </i></font></a>      A_lin[(i + 12 * (r2 + 9)) + 9] = O[i + 3 * r2];
<a name="848"><font color="#888888 "><i>  848   </i></font></a>    <b>}</b>
<a name="849"><font color="#888888 "><i>  849   </i></font></a>  <b>}</b>
<a name="850"><font color="#888888 "><i>  850   </i></font></a>
<a name="851"><font color="#888888 "><i>  851   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="852"><font color="#888888 "><i>  852   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="853"><font color="#888888 "><i>  853   </i></font></a>      b_A_lin[i + 12 * r2] = (<font color="#112266"><b>float</b></font>)I[i + 12 * r2] + A_lin[i + 12 * r2] * dt;
<a name="854"><font color="#888888 "><i>  854   </i></font></a>    <b>}</b>
<a name="855"><font color="#888888 "><i>  855   </i></font></a>  <b>}</b>
<a name="856"><font color="#888888 "><i>  856   </i></font></a>
<a name="857"><font color="#888888 "><i>  857   </i></font></a>  <font color="#117755"><i>/* process covariance matrix */</i></font>
<a name="858"><font color="#888888 "><i>  858   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,156">'AttitudeEKF:156'</a></span> if (isempty(Q)) */</i></font>
<a name="859"><font color="#888888 "><i>  859   </i></font></a>  <font color="#112266"><b>if</b></font> (!<a href="#var_Q_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q_not_empty');" target="_self"><font color="#1122aa">Q_not_empty</font></a>) <b>{</b>
<a name="860"><font color="#888888 "><i>  860   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,157">'AttitudeEKF:157'</a></span> Q=diag([ q_rotSpeed,q_rotSpeed,q_rotSpeed,... */</i></font>
<a name="861"><font color="#888888 "><i>  861   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,158">'AttitudeEKF:158'</a></span>         q_rotAcc,q_rotAcc,q_rotAcc,... */</i></font>
<a name="862"><font color="#888888 "><i>  862   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,159">'AttitudeEKF:159'</a></span>         q_acc,q_acc,q_acc,... */</i></font>
<a name="863"><font color="#888888 "><i>  863   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,160">'AttitudeEKF:160'</a></span>         q_mag,q_mag,q_mag]); */</i></font>
<a name="864"><font color="#888888 "><i>  864   </i></font></a>    v[0] = q_rotSpeed;
<a name="865"><font color="#888888 "><i>  865   </i></font></a>    v[1] = q_rotSpeed;
<a name="866"><font color="#888888 "><i>  866   </i></font></a>    v[2] = q_rotSpeed;
<a name="867"><font color="#888888 "><i>  867   </i></font></a>    v[3] = q_rotAcc;
<a name="868"><font color="#888888 "><i>  868   </i></font></a>    v[4] = q_rotAcc;
<a name="869"><font color="#888888 "><i>  869   </i></font></a>    v[5] = q_rotAcc;
<a name="870"><font color="#888888 "><i>  870   </i></font></a>    v[6] = q_acc;
<a name="871"><font color="#888888 "><i>  871   </i></font></a>    v[7] = q_acc;
<a name="872"><font color="#888888 "><i>  872   </i></font></a>    v[8] = q_acc;
<a name="873"><font color="#888888 "><i>  873   </i></font></a>    v[9] = q_mag;
<a name="874"><font color="#888888 "><i>  874   </i></font></a>    v[10] = q_mag;
<a name="875"><font color="#888888 "><i>  875   </i></font></a>    v[11] = q_mag;
<a name="876"><font color="#888888 "><i>  876   </i></font></a>    memset(&amp;<a href="#var_Q" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q');" target="_self"><font color="#1122aa">Q</font></a>[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>));
<a name="877"><font color="#888888 "><i>  877   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="878"><font color="#888888 "><i>  878   </i></font></a>      <a href="#var_Q" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q');" target="_self"><font color="#1122aa">Q</font></a>[i + 12 * i] = v[i];
<a name="879"><font color="#888888 "><i>  879   </i></font></a>    <b>}</b>
<a name="880"><font color="#888888 "><i>  880   </i></font></a>
<a name="881"><font color="#888888 "><i>  881   </i></font></a>    <a href="#var_Q_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q_not_empty');" target="_self"><font color="#1122aa">Q_not_empty</font></a> = true;
<a name="882"><font color="#888888 "><i>  882   </i></font></a>  <b>}</b>
<a name="883"><font color="#888888 "><i>  883   </i></font></a>
<a name="884"><font color="#888888 "><i>  884   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,163">'AttitudeEKF:163'</a></span> P_apr=A_lin*P_apo*A_lin'+Q; */</i></font>
<a name="885"><font color="#888888 "><i>  885   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="886"><font color="#888888 "><i>  886   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="887"><font color="#888888 "><i>  887   </i></font></a>      A_lin[r2 + 12 * i] = 0.0F;
<a name="888"><font color="#888888 "><i>  888   </i></font></a>      <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="889"><font color="#888888 "><i>  889   </i></font></a>        A_lin[r2 + 12 * i] += b_A_lin[r2 + 12 * r1] * <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r1 + 12 * i];
<a name="890"><font color="#888888 "><i>  890   </i></font></a>      <b>}</b>
<a name="891"><font color="#888888 "><i>  891   </i></font></a>    <b>}</b>
<a name="892"><font color="#888888 "><i>  892   </i></font></a>  <b>}</b>
<a name="893"><font color="#888888 "><i>  893   </i></font></a>
<a name="894"><font color="#888888 "><i>  894   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="895"><font color="#888888 "><i>  895   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="896"><font color="#888888 "><i>  896   </i></font></a>      maxval = 0.0F;
<a name="897"><font color="#888888 "><i>  897   </i></font></a>      <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="898"><font color="#888888 "><i>  898   </i></font></a>        maxval += A_lin[r2 + 12 * r1] * b_A_lin[i + 12 * r1];
<a name="899"><font color="#888888 "><i>  899   </i></font></a>      <b>}</b>
<a name="900"><font color="#888888 "><i>  900   </i></font></a>
<a name="901"><font color="#888888 "><i>  901   </i></font></a>      P_apr[r2 + 12 * i] = maxval + <a href="#var_Q" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q');" target="_self"><font color="#1122aa">Q</font></a>[r2 + 12 * i];
<a name="902"><font color="#888888 "><i>  902   </i></font></a>    <b>}</b>
<a name="903"><font color="#888888 "><i>  903   </i></font></a>  <b>}</b>
<a name="904"><font color="#888888 "><i>  904   </i></font></a>
<a name="905"><font color="#888888 "><i>  905   </i></font></a>  <font color="#117755"><i>/* % update */</i></font>
<a name="906"><font color="#888888 "><i>  906   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,167">'AttitudeEKF:167'</a></span> if zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==1 */</i></font>
<a name="907"><font color="#888888 "><i>  907   </i></font></a>  <font color="#112266"><b>if</b></font> ((zFlag[0] == 1) &amp;&amp; (zFlag[1] == 1) &amp;&amp; (zFlag[2] == 1)) <b>{</b>
<a name="908"><font color="#888888 "><i>  908   </i></font></a>    <font color="#117755"><i>/*      R=[r_gyro,0,0,0,0,0,0,0,0; */</i></font>
<a name="909"><font color="#888888 "><i>  909   </i></font></a>    <font color="#117755"><i>/*          0,r_gyro,0,0,0,0,0,0,0; */</i></font>
<a name="910"><font color="#888888 "><i>  910   </i></font></a>    <font color="#117755"><i>/*          0,0,r_gyro,0,0,0,0,0,0; */</i></font>
<a name="911"><font color="#888888 "><i>  911   </i></font></a>    <font color="#117755"><i>/*          0,0,0,r_accel,0,0,0,0,0; */</i></font>
<a name="912"><font color="#888888 "><i>  912   </i></font></a>    <font color="#117755"><i>/*          0,0,0,0,r_accel,0,0,0,0; */</i></font>
<a name="913"><font color="#888888 "><i>  913   </i></font></a>    <font color="#117755"><i>/*          0,0,0,0,0,r_accel,0,0,0; */</i></font>
<a name="914"><font color="#888888 "><i>  914   </i></font></a>    <font color="#117755"><i>/*          0,0,0,0,0,0,r_mag,0,0; */</i></font>
<a name="915"><font color="#888888 "><i>  915   </i></font></a>    <font color="#117755"><i>/*          0,0,0,0,0,0,0,r_mag,0; */</i></font>
<a name="916"><font color="#888888 "><i>  916   </i></font></a>    <font color="#117755"><i>/*          0,0,0,0,0,0,0,0,r_mag]; */</i></font>
<a name="917"><font color="#888888 "><i>  917   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,178">'AttitudeEKF:178'</a></span> R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel,r_mag,r_mag,r_mag]; */</i></font>
<a name="918"><font color="#888888 "><i>  918   </i></font></a>    <font color="#117755"><i>/* observation matrix */</i></font>
<a name="919"><font color="#888888 "><i>  919   </i></font></a>    <font color="#117755"><i>/* [zw;ze;zmk]; */</i></font>
<a name="920"><font color="#888888 "><i>  920   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,181">'AttitudeEKF:181'</a></span> H_k=[  E,     Z,      Z,    Z; */</i></font>
<a name="921"><font color="#888888 "><i>  921   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,182">'AttitudeEKF:182'</a></span>         Z,     Z,      E,    Z; */</i></font>
<a name="922"><font color="#888888 "><i>  922   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,183">'AttitudeEKF:183'</a></span>         Z,     Z,      Z,    E]; */</i></font>
<a name="923"><font color="#888888 "><i>  923   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,185">'AttitudeEKF:185'</a></span> y_k=z(1:9)-H_k*x_apr; */</i></font>
<a name="924"><font color="#888888 "><i>  924   </i></font></a>    <font color="#117755"><i>/* S_k=H_k*P_apr*H_k'+R; */</i></font>
<a name="925"><font color="#888888 "><i>  925   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,189">'AttitudeEKF:189'</a></span> S_k=H_k*P_apr*H_k'; */</i></font>
<a name="926"><font color="#888888 "><i>  926   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 9; r2++) <b>{</b>
<a name="927"><font color="#888888 "><i>  927   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="928"><font color="#888888 "><i>  928   </i></font></a>        a[r2 + 9 * i] = 0.0F;
<a name="929"><font color="#888888 "><i>  929   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="930"><font color="#888888 "><i>  930   </i></font></a>          a[r2 + 9 * i] += (<font color="#112266"><b>float</b></font>)b_a[r2 + 9 * r1] * P_apr[r1 + 12 * i];
<a name="931"><font color="#888888 "><i>  931   </i></font></a>        <b>}</b>
<a name="932"><font color="#888888 "><i>  932   </i></font></a>      <b>}</b>
<a name="933"><font color="#888888 "><i>  933   </i></font></a>
<a name="934"><font color="#888888 "><i>  934   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 9; i++) <b>{</b>
<a name="935"><font color="#888888 "><i>  935   </i></font></a>        S_k[r2 + 9 * i] = 0.0F;
<a name="936"><font color="#888888 "><i>  936   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="937"><font color="#888888 "><i>  937   </i></font></a>          S_k[r2 + 9 * i] += a[r2 + 9 * r1] * (<font color="#112266"><b>float</b></font>)b[r1 + 12 * i];
<a name="938"><font color="#888888 "><i>  938   </i></font></a>        <b>}</b>
<a name="939"><font color="#888888 "><i>  939   </i></font></a>      <b>}</b>
<a name="940"><font color="#888888 "><i>  940   </i></font></a>    <b>}</b>
<a name="941"><font color="#888888 "><i>  941   </i></font></a>
<a name="942"><font color="#888888 "><i>  942   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,190">'AttitudeEKF:190'</a></span> S_k(1:9+1:end) = S_k(1:9+1:end) + R_v; */</i></font>
<a name="943"><font color="#888888 "><i>  943   </i></font></a>    b_r_gyro[0] = r_gyro;
<a name="944"><font color="#888888 "><i>  944   </i></font></a>    b_r_gyro[1] = r_gyro;
<a name="945"><font color="#888888 "><i>  945   </i></font></a>    b_r_gyro[2] = r_gyro;
<a name="946"><font color="#888888 "><i>  946   </i></font></a>    b_r_gyro[3] = r_accel;
<a name="947"><font color="#888888 "><i>  947   </i></font></a>    b_r_gyro[4] = r_accel;
<a name="948"><font color="#888888 "><i>  948   </i></font></a>    b_r_gyro[5] = r_accel;
<a name="949"><font color="#888888 "><i>  949   </i></font></a>    b_r_gyro[6] = r_mag;
<a name="950"><font color="#888888 "><i>  950   </i></font></a>    b_r_gyro[7] = r_mag;
<a name="951"><font color="#888888 "><i>  951   </i></font></a>    b_r_gyro[8] = r_mag;
<a name="952"><font color="#888888 "><i>  952   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 9; r2++) <b>{</b>
<a name="953"><font color="#888888 "><i>  953   </i></font></a>      O[r2] = S_k[10 * r2] + b_r_gyro[r2];
<a name="954"><font color="#888888 "><i>  954   </i></font></a>    <b>}</b>
<a name="955"><font color="#888888 "><i>  955   </i></font></a>
<a name="956"><font color="#888888 "><i>  956   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 9; r2++) <b>{</b>
<a name="957"><font color="#888888 "><i>  957   </i></font></a>      S_k[10 * r2] = O[r2];
<a name="958"><font color="#888888 "><i>  958   </i></font></a>    <b>}</b>
<a name="959"><font color="#888888 "><i>  959   </i></font></a>
<a name="960"><font color="#888888 "><i>  960   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,191">'AttitudeEKF:191'</a></span> K_k=(P_apr*H_k'/(S_k)); */</i></font>
<a name="961"><font color="#888888 "><i>  961   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="962"><font color="#888888 "><i>  962   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 9; i++) <b>{</b>
<a name="963"><font color="#888888 "><i>  963   </i></font></a>        K_k[r2 + 12 * i] = 0.0F;
<a name="964"><font color="#888888 "><i>  964   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="965"><font color="#888888 "><i>  965   </i></font></a>          K_k[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)b[r1 + 12 * i];
<a name="966"><font color="#888888 "><i>  966   </i></font></a>        <b>}</b>
<a name="967"><font color="#888888 "><i>  967   </i></font></a>      <b>}</b>
<a name="968"><font color="#888888 "><i>  968   </i></font></a>    <b>}</b>
<a name="969"><font color="#888888 "><i>  969   </i></font></a>
<a name="970"><font color="#888888 "><i>  970   </i></font></a>    <a href="#fcn_mrdivide" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_mrdivide');" target="_self"><font color="#1122aa">mrdivide</font></a>(K_k, S_k);
<a name="971"><font color="#888888 "><i>  971   </i></font></a>
<a name="972"><font color="#888888 "><i>  972   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,194">'AttitudeEKF:194'</a></span> x_apo=x_apr+K_k*y_k; */</i></font>
<a name="973"><font color="#888888 "><i>  973   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 9; r2++) <b>{</b>
<a name="974"><font color="#888888 "><i>  974   </i></font></a>      maxval = 0.0F;
<a name="975"><font color="#888888 "><i>  975   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="976"><font color="#888888 "><i>  976   </i></font></a>        maxval += (<font color="#112266"><b>float</b></font>)b_a[r2 + 9 * i] * x_apr[i];
<a name="977"><font color="#888888 "><i>  977   </i></font></a>      <b>}</b>
<a name="978"><font color="#888888 "><i>  978   </i></font></a>
<a name="979"><font color="#888888 "><i>  979   </i></font></a>      O[r2] = z[r2] - maxval;
<a name="980"><font color="#888888 "><i>  980   </i></font></a>    <b>}</b>
<a name="981"><font color="#888888 "><i>  981   </i></font></a>
<a name="982"><font color="#888888 "><i>  982   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="983"><font color="#888888 "><i>  983   </i></font></a>      maxval = 0.0F;
<a name="984"><font color="#888888 "><i>  984   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 9; i++) <b>{</b>
<a name="985"><font color="#888888 "><i>  985   </i></font></a>        maxval += K_k[r2 + 12 * i] * O[i];
<a name="986"><font color="#888888 "><i>  986   </i></font></a>      <b>}</b>
<a name="987"><font color="#888888 "><i>  987   </i></font></a>
<a name="988"><font color="#888888 "><i>  988   </i></font></a>      <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[r2] = x_apr[r2] + maxval;
<a name="989"><font color="#888888 "><i>  989   </i></font></a>    <b>}</b>
<a name="990"><font color="#888888 "><i>  990   </i></font></a>
<a name="991"><font color="#888888 "><i>  991   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,195">'AttitudeEKF:195'</a></span> P_apo=(eye(12)-K_k*H_k)*P_apr; */</i></font>
<a name="992"><font color="#888888 "><i>  992   </i></font></a>    memset(&amp;I[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>));
<a name="993"><font color="#888888 "><i>  993   </i></font></a>    <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="994"><font color="#888888 "><i>  994   </i></font></a>      I[i + 12 * i] = 1;
<a name="995"><font color="#888888 "><i>  995   </i></font></a>    <b>}</b>
<a name="996"><font color="#888888 "><i>  996   </i></font></a>
<a name="997"><font color="#888888 "><i>  997   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="998"><font color="#888888 "><i>  998   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="999"><font color="#888888 "><i>  999   </i></font></a>        maxval = 0.0F;
<a name="1000"><font color="#888888 "><i> 1000   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 9; r1++) <b>{</b>
<a name="1001"><font color="#888888 "><i> 1001   </i></font></a>          maxval += K_k[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)b_a[r1 + 9 * i];
<a name="1002"><font color="#888888 "><i> 1002   </i></font></a>        <b>}</b>
<a name="1003"><font color="#888888 "><i> 1003   </i></font></a>
<a name="1004"><font color="#888888 "><i> 1004   </i></font></a>        A_lin[r2 + 12 * i] = (<font color="#112266"><b>float</b></font>)I[r2 + 12 * i] - maxval;
<a name="1005"><font color="#888888 "><i> 1005   </i></font></a>      <b>}</b>
<a name="1006"><font color="#888888 "><i> 1006   </i></font></a>    <b>}</b>
<a name="1007"><font color="#888888 "><i> 1007   </i></font></a>
<a name="1008"><font color="#888888 "><i> 1008   </i></font></a>    <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1009"><font color="#888888 "><i> 1009   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1010"><font color="#888888 "><i> 1010   </i></font></a>        <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] = 0.0F;
<a name="1011"><font color="#888888 "><i> 1011   </i></font></a>        <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1012"><font color="#888888 "><i> 1012   </i></font></a>          <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
<a name="1013"><font color="#888888 "><i> 1013   </i></font></a>        <b>}</b>
<a name="1014"><font color="#888888 "><i> 1014   </i></font></a>      <b>}</b>
<a name="1015"><font color="#888888 "><i> 1015   </i></font></a>    <b>}</b>
<a name="1016"><font color="#888888 "><i> 1016   </i></font></a>  <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="1017"><font color="#888888 "><i> 1017   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,196">'AttitudeEKF:196'</a></span> else */</i></font>
<a name="1018"><font color="#888888 "><i> 1018   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,197">'AttitudeEKF:197'</a></span> if zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==0 */</i></font>
<a name="1019"><font color="#888888 "><i> 1019   </i></font></a>    <font color="#112266"><b>if</b></font> ((zFlag[0] == 1) &amp;&amp; (zFlag[1] == 0) &amp;&amp; (zFlag[2] == 0)) <b>{</b>
<a name="1020"><font color="#888888 "><i> 1020   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,199">'AttitudeEKF:199'</a></span> R=[r_gyro,0,0; */</i></font>
<a name="1021"><font color="#888888 "><i> 1021   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,200">'AttitudeEKF:200'</a></span>             0,r_gyro,0; */</i></font>
<a name="1022"><font color="#888888 "><i> 1022   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,201">'AttitudeEKF:201'</a></span>             0,0,r_gyro]; */</i></font>
<a name="1023"><font color="#888888 "><i> 1023   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,202">'AttitudeEKF:202'</a></span> R_v=[r_gyro,r_gyro,r_gyro]; */</i></font>
<a name="1024"><font color="#888888 "><i> 1024   </i></font></a>      <font color="#117755"><i>/* observation matrix */</i></font>
<a name="1025"><font color="#888888 "><i> 1025   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,205">'AttitudeEKF:205'</a></span> H_k=[  E,     Z,      Z,    Z]; */</i></font>
<a name="1026"><font color="#888888 "><i> 1026   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,207">'AttitudeEKF:207'</a></span> y_k=z(1:3)-H_k(1:3,1:12)*x_apr; */</i></font>
<a name="1027"><font color="#888888 "><i> 1027   </i></font></a>      <font color="#117755"><i>/*  S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'+R(1:3,1:3); */</i></font>
<a name="1028"><font color="#888888 "><i> 1028   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,210">'AttitudeEKF:210'</a></span> S_k=H_k(1:3,1:12)*P_apr*H_k(1:3,1:12)'; */</i></font>
<a name="1029"><font color="#888888 "><i> 1029   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1030"><font color="#888888 "><i> 1030   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1031"><font color="#888888 "><i> 1031   </i></font></a>          b_S_k[r2 + 3 * i] = 0.0F;
<a name="1032"><font color="#888888 "><i> 1032   </i></font></a>          <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1033"><font color="#888888 "><i> 1033   </i></font></a>            b_S_k[r2 + 3 * i] += (<font color="#112266"><b>float</b></font>)c_a[r2 + 3 * r1] * P_apr[r1 + 12 * i];
<a name="1034"><font color="#888888 "><i> 1034   </i></font></a>          <b>}</b>
<a name="1035"><font color="#888888 "><i> 1035   </i></font></a>        <b>}</b>
<a name="1036"><font color="#888888 "><i> 1036   </i></font></a>
<a name="1037"><font color="#888888 "><i> 1037   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="1038"><font color="#888888 "><i> 1038   </i></font></a>          O[r2 + 3 * i] = 0.0F;
<a name="1039"><font color="#888888 "><i> 1039   </i></font></a>          <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1040"><font color="#888888 "><i> 1040   </i></font></a>            O[r2 + 3 * i] += b_S_k[r2 + 3 * r1] * (<font color="#112266"><b>float</b></font>)b_b[r1 + 12 * i];
<a name="1041"><font color="#888888 "><i> 1041   </i></font></a>          <b>}</b>
<a name="1042"><font color="#888888 "><i> 1042   </i></font></a>        <b>}</b>
<a name="1043"><font color="#888888 "><i> 1043   </i></font></a>      <b>}</b>
<a name="1044"><font color="#888888 "><i> 1044   </i></font></a>
<a name="1045"><font color="#888888 "><i> 1045   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,211">'AttitudeEKF:211'</a></span> S_k(1:3+1:end) = S_k(1:3+1:end) + R_v; */</i></font>
<a name="1046"><font color="#888888 "><i> 1046   </i></font></a>      c_r_gyro[0] = r_gyro;
<a name="1047"><font color="#888888 "><i> 1047   </i></font></a>      c_r_gyro[1] = r_gyro;
<a name="1048"><font color="#888888 "><i> 1048   </i></font></a>      c_r_gyro[2] = r_gyro;
<a name="1049"><font color="#888888 "><i> 1049   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1050"><font color="#888888 "><i> 1050   </i></font></a>        b_muk[r2] = O[r2 &lt;&lt; 2] + c_r_gyro[r2];
<a name="1051"><font color="#888888 "><i> 1051   </i></font></a>      <b>}</b>
<a name="1052"><font color="#888888 "><i> 1052   </i></font></a>
<a name="1053"><font color="#888888 "><i> 1053   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1054"><font color="#888888 "><i> 1054   </i></font></a>        O[r2 &lt;&lt; 2] = b_muk[r2];
<a name="1055"><font color="#888888 "><i> 1055   </i></font></a>      <b>}</b>
<a name="1056"><font color="#888888 "><i> 1056   </i></font></a>
<a name="1057"><font color="#888888 "><i> 1057   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,212">'AttitudeEKF:212'</a></span> K_k=(P_apr*H_k(1:3,1:12)'/(S_k)); */</i></font>
<a name="1058"><font color="#888888 "><i> 1058   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1059"><font color="#888888 "><i> 1059   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="1060"><font color="#888888 "><i> 1060   </i></font></a>          y[r2 + 12 * i] = 0.0F;
<a name="1061"><font color="#888888 "><i> 1061   </i></font></a>          <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1062"><font color="#888888 "><i> 1062   </i></font></a>            y[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)b_b[r1 + 12 * i];
<a name="1063"><font color="#888888 "><i> 1063   </i></font></a>          <b>}</b>
<a name="1064"><font color="#888888 "><i> 1064   </i></font></a>        <b>}</b>
<a name="1065"><font color="#888888 "><i> 1065   </i></font></a>      <b>}</b>
<a name="1066"><font color="#888888 "><i> 1066   </i></font></a>
<a name="1067"><font color="#888888 "><i> 1067   </i></font></a>      r1 = 0;
<a name="1068"><font color="#888888 "><i> 1068   </i></font></a>      r2 = 1;
<a name="1069"><font color="#888888 "><i> 1069   </i></font></a>      r3 = 2;
<a name="1070"><font color="#888888 "><i> 1070   </i></font></a>      maxval = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(O[0]);
<a name="1071"><font color="#888888 "><i> 1071   </i></font></a>      a21 = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(O[1]);
<a name="1072"><font color="#888888 "><i> 1072   </i></font></a>      <font color="#112266"><b>if</b></font> (a21 &gt; maxval) <b>{</b>
<a name="1073"><font color="#888888 "><i> 1073   </i></font></a>        maxval = a21;
<a name="1074"><font color="#888888 "><i> 1074   </i></font></a>        r1 = 1;
<a name="1075"><font color="#888888 "><i> 1075   </i></font></a>        r2 = 0;
<a name="1076"><font color="#888888 "><i> 1076   </i></font></a>      <b>}</b>
<a name="1077"><font color="#888888 "><i> 1077   </i></font></a>
<a name="1078"><font color="#888888 "><i> 1078   </i></font></a>      <font color="#112266"><b>if</b></font> ((<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(O[2]) &gt; maxval) <b>{</b>
<a name="1079"><font color="#888888 "><i> 1079   </i></font></a>        r1 = 2;
<a name="1080"><font color="#888888 "><i> 1080   </i></font></a>        r2 = 1;
<a name="1081"><font color="#888888 "><i> 1081   </i></font></a>        r3 = 0;
<a name="1082"><font color="#888888 "><i> 1082   </i></font></a>      <b>}</b>
<a name="1083"><font color="#888888 "><i> 1083   </i></font></a>
<a name="1084"><font color="#888888 "><i> 1084   </i></font></a>      O[r2] /= O[r1];
<a name="1085"><font color="#888888 "><i> 1085   </i></font></a>      O[r3] /= O[r1];
<a name="1086"><font color="#888888 "><i> 1086   </i></font></a>      O[3 + r2] -= O[r2] * O[3 + r1];
<a name="1087"><font color="#888888 "><i> 1087   </i></font></a>      O[3 + r3] -= O[r3] * O[3 + r1];
<a name="1088"><font color="#888888 "><i> 1088   </i></font></a>      O[6 + r2] -= O[r2] * O[6 + r1];
<a name="1089"><font color="#888888 "><i> 1089   </i></font></a>      O[6 + r3] -= O[r3] * O[6 + r1];
<a name="1090"><font color="#888888 "><i> 1090   </i></font></a>      <font color="#112266"><b>if</b></font> ((<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(O[3 + r3]) &gt; (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)fabs(O[3 + r2])) <b>{</b>
<a name="1091"><font color="#888888 "><i> 1091   </i></font></a>        i = r2;
<a name="1092"><font color="#888888 "><i> 1092   </i></font></a>        r2 = r3;
<a name="1093"><font color="#888888 "><i> 1093   </i></font></a>        r3 = i;
<a name="1094"><font color="#888888 "><i> 1094   </i></font></a>      <b>}</b>
<a name="1095"><font color="#888888 "><i> 1095   </i></font></a>
<a name="1096"><font color="#888888 "><i> 1096   </i></font></a>      O[3 + r3] /= O[3 + r2];
<a name="1097"><font color="#888888 "><i> 1097   </i></font></a>      O[6 + r3] -= O[3 + r3] * O[6 + r2];
<a name="1098"><font color="#888888 "><i> 1098   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1099"><font color="#888888 "><i> 1099   </i></font></a>        b_S_k[i + 12 * r1] = y[i] / O[r1];
<a name="1100"><font color="#888888 "><i> 1100   </i></font></a>        b_S_k[i + 12 * r2] = y[12 + i] - b_S_k[i + 12 * r1] * O[3 + r1];
<a name="1101"><font color="#888888 "><i> 1101   </i></font></a>        b_S_k[i + 12 * r3] = y[24 + i] - b_S_k[i + 12 * r1] * O[6 + r1];
<a name="1102"><font color="#888888 "><i> 1102   </i></font></a>        b_S_k[i + 12 * r2] /= O[3 + r2];
<a name="1103"><font color="#888888 "><i> 1103   </i></font></a>        b_S_k[i + 12 * r3] -= b_S_k[i + 12 * r2] * O[6 + r2];
<a name="1104"><font color="#888888 "><i> 1104   </i></font></a>        b_S_k[i + 12 * r3] /= O[6 + r3];
<a name="1105"><font color="#888888 "><i> 1105   </i></font></a>        b_S_k[i + 12 * r2] -= b_S_k[i + 12 * r3] * O[3 + r3];
<a name="1106"><font color="#888888 "><i> 1106   </i></font></a>        b_S_k[i + 12 * r1] -= b_S_k[i + 12 * r3] * O[r3];
<a name="1107"><font color="#888888 "><i> 1107   </i></font></a>        b_S_k[i + 12 * r1] -= b_S_k[i + 12 * r2] * O[r2];
<a name="1108"><font color="#888888 "><i> 1108   </i></font></a>      <b>}</b>
<a name="1109"><font color="#888888 "><i> 1109   </i></font></a>
<a name="1110"><font color="#888888 "><i> 1110   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,215">'AttitudeEKF:215'</a></span> x_apo=x_apr+K_k*y_k; */</i></font>
<a name="1111"><font color="#888888 "><i> 1111   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1112"><font color="#888888 "><i> 1112   </i></font></a>        maxval = 0.0F;
<a name="1113"><font color="#888888 "><i> 1113   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1114"><font color="#888888 "><i> 1114   </i></font></a>          maxval += (<font color="#112266"><b>float</b></font>)c_a[r2 + 3 * i] * x_apr[i];
<a name="1115"><font color="#888888 "><i> 1115   </i></font></a>        <b>}</b>
<a name="1116"><font color="#888888 "><i> 1116   </i></font></a>
<a name="1117"><font color="#888888 "><i> 1117   </i></font></a>        b_muk[r2] = z[r2] - maxval;
<a name="1118"><font color="#888888 "><i> 1118   </i></font></a>      <b>}</b>
<a name="1119"><font color="#888888 "><i> 1119   </i></font></a>
<a name="1120"><font color="#888888 "><i> 1120   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1121"><font color="#888888 "><i> 1121   </i></font></a>        maxval = 0.0F;
<a name="1122"><font color="#888888 "><i> 1122   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="1123"><font color="#888888 "><i> 1123   </i></font></a>          maxval += b_S_k[r2 + 12 * i] * b_muk[i];
<a name="1124"><font color="#888888 "><i> 1124   </i></font></a>        <b>}</b>
<a name="1125"><font color="#888888 "><i> 1125   </i></font></a>
<a name="1126"><font color="#888888 "><i> 1126   </i></font></a>        <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[r2] = x_apr[r2] + maxval;
<a name="1127"><font color="#888888 "><i> 1127   </i></font></a>      <b>}</b>
<a name="1128"><font color="#888888 "><i> 1128   </i></font></a>
<a name="1129"><font color="#888888 "><i> 1129   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,216">'AttitudeEKF:216'</a></span> P_apo=(eye(12)-K_k*H_k(1:3,1:12))*P_apr; */</i></font>
<a name="1130"><font color="#888888 "><i> 1130   </i></font></a>      memset(&amp;I[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>));
<a name="1131"><font color="#888888 "><i> 1131   </i></font></a>      <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1132"><font color="#888888 "><i> 1132   </i></font></a>        I[i + 12 * i] = 1;
<a name="1133"><font color="#888888 "><i> 1133   </i></font></a>      <b>}</b>
<a name="1134"><font color="#888888 "><i> 1134   </i></font></a>
<a name="1135"><font color="#888888 "><i> 1135   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1136"><font color="#888888 "><i> 1136   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1137"><font color="#888888 "><i> 1137   </i></font></a>          maxval = 0.0F;
<a name="1138"><font color="#888888 "><i> 1138   </i></font></a>          <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 3; r1++) <b>{</b>
<a name="1139"><font color="#888888 "><i> 1139   </i></font></a>            maxval += b_S_k[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)c_a[r1 + 3 * i];
<a name="1140"><font color="#888888 "><i> 1140   </i></font></a>          <b>}</b>
<a name="1141"><font color="#888888 "><i> 1141   </i></font></a>
<a name="1142"><font color="#888888 "><i> 1142   </i></font></a>          A_lin[r2 + 12 * i] = (<font color="#112266"><b>float</b></font>)I[r2 + 12 * i] - maxval;
<a name="1143"><font color="#888888 "><i> 1143   </i></font></a>        <b>}</b>
<a name="1144"><font color="#888888 "><i> 1144   </i></font></a>      <b>}</b>
<a name="1145"><font color="#888888 "><i> 1145   </i></font></a>
<a name="1146"><font color="#888888 "><i> 1146   </i></font></a>      <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1147"><font color="#888888 "><i> 1147   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1148"><font color="#888888 "><i> 1148   </i></font></a>          <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] = 0.0F;
<a name="1149"><font color="#888888 "><i> 1149   </i></font></a>          <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1150"><font color="#888888 "><i> 1150   </i></font></a>            <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
<a name="1151"><font color="#888888 "><i> 1151   </i></font></a>          <b>}</b>
<a name="1152"><font color="#888888 "><i> 1152   </i></font></a>        <b>}</b>
<a name="1153"><font color="#888888 "><i> 1153   </i></font></a>      <b>}</b>
<a name="1154"><font color="#888888 "><i> 1154   </i></font></a>    <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="1155"><font color="#888888 "><i> 1155   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,217">'AttitudeEKF:217'</a></span> else */</i></font>
<a name="1156"><font color="#888888 "><i> 1156   </i></font></a>      <font color="#117755"><i>/* <span><a href="1,218">'AttitudeEKF:218'</a></span> if  zFlag(1)==1&&zFlag(2)==1&&zFlag(3)==0 */</i></font>
<a name="1157"><font color="#888888 "><i> 1157   </i></font></a>      <font color="#112266"><b>if</b></font> ((zFlag[0] == 1) &amp;&amp; (zFlag[1] == 1) &amp;&amp; (zFlag[2] == 0)) <b>{</b>
<a name="1158"><font color="#888888 "><i> 1158   </i></font></a>        <font color="#117755"><i>/*              R=[r_gyro,0,0,0,0,0; */</i></font>
<a name="1159"><font color="#888888 "><i> 1159   </i></font></a>        <font color="#117755"><i>/*                  0,r_gyro,0,0,0,0; */</i></font>
<a name="1160"><font color="#888888 "><i> 1160   </i></font></a>        <font color="#117755"><i>/*                  0,0,r_gyro,0,0,0; */</i></font>
<a name="1161"><font color="#888888 "><i> 1161   </i></font></a>        <font color="#117755"><i>/*                  0,0,0,r_accel,0,0; */</i></font>
<a name="1162"><font color="#888888 "><i> 1162   </i></font></a>        <font color="#117755"><i>/*                  0,0,0,0,r_accel,0; */</i></font>
<a name="1163"><font color="#888888 "><i> 1163   </i></font></a>        <font color="#117755"><i>/*                  0,0,0,0,0,r_accel]; */</i></font>
<a name="1164"><font color="#888888 "><i> 1164   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,227">'AttitudeEKF:227'</a></span> R_v=[r_gyro,r_gyro,r_gyro,r_accel,r_accel,r_accel]; */</i></font>
<a name="1165"><font color="#888888 "><i> 1165   </i></font></a>        <font color="#117755"><i>/* observation matrix */</i></font>
<a name="1166"><font color="#888888 "><i> 1166   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,230">'AttitudeEKF:230'</a></span> H_k=[  E,     Z,      Z,    Z; */</i></font>
<a name="1167"><font color="#888888 "><i> 1167   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,231">'AttitudeEKF:231'</a></span>                 Z,     Z,      E,    Z]; */</i></font>
<a name="1168"><font color="#888888 "><i> 1168   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,233">'AttitudeEKF:233'</a></span> y_k=z(1:6)-H_k(1:6,1:12)*x_apr; */</i></font>
<a name="1169"><font color="#888888 "><i> 1169   </i></font></a>        <font color="#117755"><i>/*  S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */</i></font>
<a name="1170"><font color="#888888 "><i> 1170   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,236">'AttitudeEKF:236'</a></span> S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */</i></font>
<a name="1171"><font color="#888888 "><i> 1171   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1172"><font color="#888888 "><i> 1172   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1173"><font color="#888888 "><i> 1173   </i></font></a>            d_a[r2 + 6 * i] = 0.0F;
<a name="1174"><font color="#888888 "><i> 1174   </i></font></a>            <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1175"><font color="#888888 "><i> 1175   </i></font></a>              d_a[r2 + 6 * i] += (<font color="#112266"><b>float</b></font>)e_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
<a name="1176"><font color="#888888 "><i> 1176   </i></font></a>            <b>}</b>
<a name="1177"><font color="#888888 "><i> 1177   </i></font></a>          <b>}</b>
<a name="1178"><font color="#888888 "><i> 1178   </i></font></a>
<a name="1179"><font color="#888888 "><i> 1179   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1180"><font color="#888888 "><i> 1180   </i></font></a>            b_S_k[r2 + 6 * i] = 0.0F;
<a name="1181"><font color="#888888 "><i> 1181   </i></font></a>            <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1182"><font color="#888888 "><i> 1182   </i></font></a>              b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (<font color="#112266"><b>float</b></font>)c_b[r1 + 12 * i];
<a name="1183"><font color="#888888 "><i> 1183   </i></font></a>            <b>}</b>
<a name="1184"><font color="#888888 "><i> 1184   </i></font></a>          <b>}</b>
<a name="1185"><font color="#888888 "><i> 1185   </i></font></a>        <b>}</b>
<a name="1186"><font color="#888888 "><i> 1186   </i></font></a>
<a name="1187"><font color="#888888 "><i> 1187   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,237">'AttitudeEKF:237'</a></span> S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */</i></font>
<a name="1188"><font color="#888888 "><i> 1188   </i></font></a>        d_r_gyro[0] = r_gyro;
<a name="1189"><font color="#888888 "><i> 1189   </i></font></a>        d_r_gyro[1] = r_gyro;
<a name="1190"><font color="#888888 "><i> 1190   </i></font></a>        d_r_gyro[2] = r_gyro;
<a name="1191"><font color="#888888 "><i> 1191   </i></font></a>        d_r_gyro[3] = r_accel;
<a name="1192"><font color="#888888 "><i> 1192   </i></font></a>        d_r_gyro[4] = r_accel;
<a name="1193"><font color="#888888 "><i> 1193   </i></font></a>        d_r_gyro[5] = r_accel;
<a name="1194"><font color="#888888 "><i> 1194   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1195"><font color="#888888 "><i> 1195   </i></font></a>          c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
<a name="1196"><font color="#888888 "><i> 1196   </i></font></a>        <b>}</b>
<a name="1197"><font color="#888888 "><i> 1197   </i></font></a>
<a name="1198"><font color="#888888 "><i> 1198   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1199"><font color="#888888 "><i> 1199   </i></font></a>          b_S_k[7 * r2] = c_S_k[r2];
<a name="1200"><font color="#888888 "><i> 1200   </i></font></a>        <b>}</b>
<a name="1201"><font color="#888888 "><i> 1201   </i></font></a>
<a name="1202"><font color="#888888 "><i> 1202   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,238">'AttitudeEKF:238'</a></span> K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */</i></font>
<a name="1203"><font color="#888888 "><i> 1203   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1204"><font color="#888888 "><i> 1204   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1205"><font color="#888888 "><i> 1205   </i></font></a>            b_K_k[r2 + 12 * i] = 0.0F;
<a name="1206"><font color="#888888 "><i> 1206   </i></font></a>            <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1207"><font color="#888888 "><i> 1207   </i></font></a>              b_K_k[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)c_b[r1 + 12 * i];
<a name="1208"><font color="#888888 "><i> 1208   </i></font></a>            <b>}</b>
<a name="1209"><font color="#888888 "><i> 1209   </i></font></a>          <b>}</b>
<a name="1210"><font color="#888888 "><i> 1210   </i></font></a>        <b>}</b>
<a name="1211"><font color="#888888 "><i> 1211   </i></font></a>
<a name="1212"><font color="#888888 "><i> 1212   </i></font></a>        <a href="#fcn_b_mrdivide" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_mrdivide');" target="_self"><font color="#1122aa">b_mrdivide</font></a>(b_K_k, b_S_k);
<a name="1213"><font color="#888888 "><i> 1213   </i></font></a>
<a name="1214"><font color="#888888 "><i> 1214   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,241">'AttitudeEKF:241'</a></span> x_apo=x_apr+K_k*y_k; */</i></font>
<a name="1215"><font color="#888888 "><i> 1215   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1216"><font color="#888888 "><i> 1216   </i></font></a>          maxval = 0.0F;
<a name="1217"><font color="#888888 "><i> 1217   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1218"><font color="#888888 "><i> 1218   </i></font></a>            maxval += (<font color="#112266"><b>float</b></font>)e_a[r2 + 6 * i] * x_apr[i];
<a name="1219"><font color="#888888 "><i> 1219   </i></font></a>          <b>}</b>
<a name="1220"><font color="#888888 "><i> 1220   </i></font></a>
<a name="1221"><font color="#888888 "><i> 1221   </i></font></a>          d_r_gyro[r2] = z[r2] - maxval;
<a name="1222"><font color="#888888 "><i> 1222   </i></font></a>        <b>}</b>
<a name="1223"><font color="#888888 "><i> 1223   </i></font></a>
<a name="1224"><font color="#888888 "><i> 1224   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1225"><font color="#888888 "><i> 1225   </i></font></a>          maxval = 0.0F;
<a name="1226"><font color="#888888 "><i> 1226   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1227"><font color="#888888 "><i> 1227   </i></font></a>            maxval += b_K_k[r2 + 12 * i] * d_r_gyro[i];
<a name="1228"><font color="#888888 "><i> 1228   </i></font></a>          <b>}</b>
<a name="1229"><font color="#888888 "><i> 1229   </i></font></a>
<a name="1230"><font color="#888888 "><i> 1230   </i></font></a>          <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[r2] = x_apr[r2] + maxval;
<a name="1231"><font color="#888888 "><i> 1231   </i></font></a>        <b>}</b>
<a name="1232"><font color="#888888 "><i> 1232   </i></font></a>
<a name="1233"><font color="#888888 "><i> 1233   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,242">'AttitudeEKF:242'</a></span> P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */</i></font>
<a name="1234"><font color="#888888 "><i> 1234   </i></font></a>        memset(&amp;I[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>));
<a name="1235"><font color="#888888 "><i> 1235   </i></font></a>        <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1236"><font color="#888888 "><i> 1236   </i></font></a>          I[i + 12 * i] = 1;
<a name="1237"><font color="#888888 "><i> 1237   </i></font></a>        <b>}</b>
<a name="1238"><font color="#888888 "><i> 1238   </i></font></a>
<a name="1239"><font color="#888888 "><i> 1239   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1240"><font color="#888888 "><i> 1240   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1241"><font color="#888888 "><i> 1241   </i></font></a>            maxval = 0.0F;
<a name="1242"><font color="#888888 "><i> 1242   </i></font></a>            <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 6; r1++) <b>{</b>
<a name="1243"><font color="#888888 "><i> 1243   </i></font></a>              maxval += b_K_k[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)e_a[r1 + 6 * i];
<a name="1244"><font color="#888888 "><i> 1244   </i></font></a>            <b>}</b>
<a name="1245"><font color="#888888 "><i> 1245   </i></font></a>
<a name="1246"><font color="#888888 "><i> 1246   </i></font></a>            A_lin[r2 + 12 * i] = (<font color="#112266"><b>float</b></font>)I[r2 + 12 * i] - maxval;
<a name="1247"><font color="#888888 "><i> 1247   </i></font></a>          <b>}</b>
<a name="1248"><font color="#888888 "><i> 1248   </i></font></a>        <b>}</b>
<a name="1249"><font color="#888888 "><i> 1249   </i></font></a>
<a name="1250"><font color="#888888 "><i> 1250   </i></font></a>        <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1251"><font color="#888888 "><i> 1251   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1252"><font color="#888888 "><i> 1252   </i></font></a>            <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] = 0.0F;
<a name="1253"><font color="#888888 "><i> 1253   </i></font></a>            <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1254"><font color="#888888 "><i> 1254   </i></font></a>              <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
<a name="1255"><font color="#888888 "><i> 1255   </i></font></a>            <b>}</b>
<a name="1256"><font color="#888888 "><i> 1256   </i></font></a>          <b>}</b>
<a name="1257"><font color="#888888 "><i> 1257   </i></font></a>        <b>}</b>
<a name="1258"><font color="#888888 "><i> 1258   </i></font></a>      <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="1259"><font color="#888888 "><i> 1259   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,243">'AttitudeEKF:243'</a></span> else */</i></font>
<a name="1260"><font color="#888888 "><i> 1260   </i></font></a>        <font color="#117755"><i>/* <span><a href="1,244">'AttitudeEKF:244'</a></span> if  zFlag(1)==1&&zFlag(2)==0&&zFlag(3)==1 */</i></font>
<a name="1261"><font color="#888888 "><i> 1261   </i></font></a>        <font color="#112266"><b>if</b></font> ((zFlag[0] == 1) &amp;&amp; (zFlag[1] == 0) &amp;&amp; (zFlag[2] == 1)) <b>{</b>
<a name="1262"><font color="#888888 "><i> 1262   </i></font></a>          <font color="#117755"><i>/*                  R=[r_gyro,0,0,0,0,0; */</i></font>
<a name="1263"><font color="#888888 "><i> 1263   </i></font></a>          <font color="#117755"><i>/*                      0,r_gyro,0,0,0,0; */</i></font>
<a name="1264"><font color="#888888 "><i> 1264   </i></font></a>          <font color="#117755"><i>/*                      0,0,r_gyro,0,0,0; */</i></font>
<a name="1265"><font color="#888888 "><i> 1265   </i></font></a>          <font color="#117755"><i>/*                      0,0,0,r_mag,0,0; */</i></font>
<a name="1266"><font color="#888888 "><i> 1266   </i></font></a>          <font color="#117755"><i>/*                      0,0,0,0,r_mag,0; */</i></font>
<a name="1267"><font color="#888888 "><i> 1267   </i></font></a>          <font color="#117755"><i>/*                      0,0,0,0,0,r_mag]; */</i></font>
<a name="1268"><font color="#888888 "><i> 1268   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,251">'AttitudeEKF:251'</a></span> R_v=[r_gyro,r_gyro,r_gyro,r_mag,r_mag,r_mag]; */</i></font>
<a name="1269"><font color="#888888 "><i> 1269   </i></font></a>          <font color="#117755"><i>/* observation matrix */</i></font>
<a name="1270"><font color="#888888 "><i> 1270   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,254">'AttitudeEKF:254'</a></span> H_k=[  E,     Z,      Z,    Z; */</i></font>
<a name="1271"><font color="#888888 "><i> 1271   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,255">'AttitudeEKF:255'</a></span>                     Z,     Z,      Z,    E]; */</i></font>
<a name="1272"><font color="#888888 "><i> 1272   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,257">'AttitudeEKF:257'</a></span> y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apr; */</i></font>
<a name="1273"><font color="#888888 "><i> 1273   </i></font></a>          <font color="#117755"><i>/* S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'+R(1:6,1:6); */</i></font>
<a name="1274"><font color="#888888 "><i> 1274   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,260">'AttitudeEKF:260'</a></span> S_k=H_k(1:6,1:12)*P_apr*H_k(1:6,1:12)'; */</i></font>
<a name="1275"><font color="#888888 "><i> 1275   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1276"><font color="#888888 "><i> 1276   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1277"><font color="#888888 "><i> 1277   </i></font></a>              d_a[r2 + 6 * i] = 0.0F;
<a name="1278"><font color="#888888 "><i> 1278   </i></font></a>              <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1279"><font color="#888888 "><i> 1279   </i></font></a>                d_a[r2 + 6 * i] += (<font color="#112266"><b>float</b></font>)f_a[r2 + 6 * r1] * P_apr[r1 + 12 * i];
<a name="1280"><font color="#888888 "><i> 1280   </i></font></a>              <b>}</b>
<a name="1281"><font color="#888888 "><i> 1281   </i></font></a>            <b>}</b>
<a name="1282"><font color="#888888 "><i> 1282   </i></font></a>
<a name="1283"><font color="#888888 "><i> 1283   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1284"><font color="#888888 "><i> 1284   </i></font></a>              b_S_k[r2 + 6 * i] = 0.0F;
<a name="1285"><font color="#888888 "><i> 1285   </i></font></a>              <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1286"><font color="#888888 "><i> 1286   </i></font></a>                b_S_k[r2 + 6 * i] += d_a[r2 + 6 * r1] * (<font color="#112266"><b>float</b></font>)d_b[r1 + 12 * i];
<a name="1287"><font color="#888888 "><i> 1287   </i></font></a>              <b>}</b>
<a name="1288"><font color="#888888 "><i> 1288   </i></font></a>            <b>}</b>
<a name="1289"><font color="#888888 "><i> 1289   </i></font></a>          <b>}</b>
<a name="1290"><font color="#888888 "><i> 1290   </i></font></a>
<a name="1291"><font color="#888888 "><i> 1291   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,261">'AttitudeEKF:261'</a></span> S_k(1:6+1:end) = S_k(1:6+1:end) + R_v; */</i></font>
<a name="1292"><font color="#888888 "><i> 1292   </i></font></a>          d_r_gyro[0] = r_gyro;
<a name="1293"><font color="#888888 "><i> 1293   </i></font></a>          d_r_gyro[1] = r_gyro;
<a name="1294"><font color="#888888 "><i> 1294   </i></font></a>          d_r_gyro[2] = r_gyro;
<a name="1295"><font color="#888888 "><i> 1295   </i></font></a>          d_r_gyro[3] = r_mag;
<a name="1296"><font color="#888888 "><i> 1296   </i></font></a>          d_r_gyro[4] = r_mag;
<a name="1297"><font color="#888888 "><i> 1297   </i></font></a>          d_r_gyro[5] = r_mag;
<a name="1298"><font color="#888888 "><i> 1298   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1299"><font color="#888888 "><i> 1299   </i></font></a>            c_S_k[r2] = b_S_k[7 * r2] + d_r_gyro[r2];
<a name="1300"><font color="#888888 "><i> 1300   </i></font></a>          <b>}</b>
<a name="1301"><font color="#888888 "><i> 1301   </i></font></a>
<a name="1302"><font color="#888888 "><i> 1302   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1303"><font color="#888888 "><i> 1303   </i></font></a>            b_S_k[7 * r2] = c_S_k[r2];
<a name="1304"><font color="#888888 "><i> 1304   </i></font></a>          <b>}</b>
<a name="1305"><font color="#888888 "><i> 1305   </i></font></a>
<a name="1306"><font color="#888888 "><i> 1306   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,262">'AttitudeEKF:262'</a></span> K_k=(P_apr*H_k(1:6,1:12)'/(S_k)); */</i></font>
<a name="1307"><font color="#888888 "><i> 1307   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1308"><font color="#888888 "><i> 1308   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1309"><font color="#888888 "><i> 1309   </i></font></a>              b_K_k[r2 + 12 * i] = 0.0F;
<a name="1310"><font color="#888888 "><i> 1310   </i></font></a>              <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1311"><font color="#888888 "><i> 1311   </i></font></a>                b_K_k[r2 + 12 * i] += P_apr[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)d_b[r1 + 12 *
<a name="1312"><font color="#888888 "><i> 1312   </i></font></a>                  i];
<a name="1313"><font color="#888888 "><i> 1313   </i></font></a>              <b>}</b>
<a name="1314"><font color="#888888 "><i> 1314   </i></font></a>            <b>}</b>
<a name="1315"><font color="#888888 "><i> 1315   </i></font></a>          <b>}</b>
<a name="1316"><font color="#888888 "><i> 1316   </i></font></a>
<a name="1317"><font color="#888888 "><i> 1317   </i></font></a>          <a href="#fcn_b_mrdivide" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_b_mrdivide');" target="_self"><font color="#1122aa">b_mrdivide</font></a>(b_K_k, b_S_k);
<a name="1318"><font color="#888888 "><i> 1318   </i></font></a>
<a name="1319"><font color="#888888 "><i> 1319   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,265">'AttitudeEKF:265'</a></span> x_apo=x_apr+K_k*y_k; */</i></font>
<a name="1320"><font color="#888888 "><i> 1320   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1321"><font color="#888888 "><i> 1321   </i></font></a>            d_r_gyro[r2] = z[r2];
<a name="1322"><font color="#888888 "><i> 1322   </i></font></a>          <b>}</b>
<a name="1323"><font color="#888888 "><i> 1323   </i></font></a>
<a name="1324"><font color="#888888 "><i> 1324   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1325"><font color="#888888 "><i> 1325   </i></font></a>            d_r_gyro[r2 + 3] = z[6 + r2];
<a name="1326"><font color="#888888 "><i> 1326   </i></font></a>          <b>}</b>
<a name="1327"><font color="#888888 "><i> 1327   </i></font></a>
<a name="1328"><font color="#888888 "><i> 1328   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 6; r2++) <b>{</b>
<a name="1329"><font color="#888888 "><i> 1329   </i></font></a>            c_S_k[r2] = 0.0F;
<a name="1330"><font color="#888888 "><i> 1330   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1331"><font color="#888888 "><i> 1331   </i></font></a>              c_S_k[r2] += (<font color="#112266"><b>float</b></font>)f_a[r2 + 6 * i] * x_apr[i];
<a name="1332"><font color="#888888 "><i> 1332   </i></font></a>            <b>}</b>
<a name="1333"><font color="#888888 "><i> 1333   </i></font></a>
<a name="1334"><font color="#888888 "><i> 1334   </i></font></a>            b_z[r2] = d_r_gyro[r2] - c_S_k[r2];
<a name="1335"><font color="#888888 "><i> 1335   </i></font></a>          <b>}</b>
<a name="1336"><font color="#888888 "><i> 1336   </i></font></a>
<a name="1337"><font color="#888888 "><i> 1337   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1338"><font color="#888888 "><i> 1338   </i></font></a>            maxval = 0.0F;
<a name="1339"><font color="#888888 "><i> 1339   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 6; i++) <b>{</b>
<a name="1340"><font color="#888888 "><i> 1340   </i></font></a>              maxval += b_K_k[r2 + 12 * i] * b_z[i];
<a name="1341"><font color="#888888 "><i> 1341   </i></font></a>            <b>}</b>
<a name="1342"><font color="#888888 "><i> 1342   </i></font></a>
<a name="1343"><font color="#888888 "><i> 1343   </i></font></a>            <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[r2] = x_apr[r2] + maxval;
<a name="1344"><font color="#888888 "><i> 1344   </i></font></a>          <b>}</b>
<a name="1345"><font color="#888888 "><i> 1345   </i></font></a>
<a name="1346"><font color="#888888 "><i> 1346   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,266">'AttitudeEKF:266'</a></span> P_apo=(eye(12)-K_k*H_k(1:6,1:12))*P_apr; */</i></font>
<a name="1347"><font color="#888888 "><i> 1347   </i></font></a>          memset(&amp;I[0], 0, 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>signed</b></font> <font color="#112266"><b>char</b></font>));
<a name="1348"><font color="#888888 "><i> 1348   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1349"><font color="#888888 "><i> 1349   </i></font></a>            I[i + 12 * i] = 1;
<a name="1350"><font color="#888888 "><i> 1350   </i></font></a>          <b>}</b>
<a name="1351"><font color="#888888 "><i> 1351   </i></font></a>
<a name="1352"><font color="#888888 "><i> 1352   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1353"><font color="#888888 "><i> 1353   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1354"><font color="#888888 "><i> 1354   </i></font></a>              maxval = 0.0F;
<a name="1355"><font color="#888888 "><i> 1355   </i></font></a>              <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 6; r1++) <b>{</b>
<a name="1356"><font color="#888888 "><i> 1356   </i></font></a>                maxval += b_K_k[r2 + 12 * r1] * (<font color="#112266"><b>float</b></font>)f_a[r1 + 6 * i];
<a name="1357"><font color="#888888 "><i> 1357   </i></font></a>              <b>}</b>
<a name="1358"><font color="#888888 "><i> 1358   </i></font></a>
<a name="1359"><font color="#888888 "><i> 1359   </i></font></a>              A_lin[r2 + 12 * i] = (<font color="#112266"><b>float</b></font>)I[r2 + 12 * i] - maxval;
<a name="1360"><font color="#888888 "><i> 1360   </i></font></a>            <b>}</b>
<a name="1361"><font color="#888888 "><i> 1361   </i></font></a>          <b>}</b>
<a name="1362"><font color="#888888 "><i> 1362   </i></font></a>
<a name="1363"><font color="#888888 "><i> 1363   </i></font></a>          <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 12; r2++) <b>{</b>
<a name="1364"><font color="#888888 "><i> 1364   </i></font></a>            <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1365"><font color="#888888 "><i> 1365   </i></font></a>              <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] = 0.0F;
<a name="1366"><font color="#888888 "><i> 1366   </i></font></a>              <font color="#112266"><b>for</b></font> (r1 = 0; r1 &lt; 12; r1++) <b>{</b>
<a name="1367"><font color="#888888 "><i> 1367   </i></font></a>                <a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[r2 + 12 * i] += A_lin[r2 + 12 * r1] * P_apr[r1 + 12 * i];
<a name="1368"><font color="#888888 "><i> 1368   </i></font></a>              <b>}</b>
<a name="1369"><font color="#888888 "><i> 1369   </i></font></a>            <b>}</b>
<a name="1370"><font color="#888888 "><i> 1370   </i></font></a>          <b>}</b>
<a name="1371"><font color="#888888 "><i> 1371   </i></font></a>        <b>}</b> <font color="#112266"><b>else</b></font> <b>{</b>
<a name="1372"><font color="#888888 "><i> 1372   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,267">'AttitudeEKF:267'</a></span> else */</i></font>
<a name="1373"><font color="#888888 "><i> 1373   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,268">'AttitudeEKF:268'</a></span> x_apo=x_apr; */</i></font>
<a name="1374"><font color="#888888 "><i> 1374   </i></font></a>          <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1375"><font color="#888888 "><i> 1375   </i></font></a>            <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[i] = x_apr[i];
<a name="1376"><font color="#888888 "><i> 1376   </i></font></a>          <b>}</b>
<a name="1377"><font color="#888888 "><i> 1377   </i></font></a>
<a name="1378"><font color="#888888 "><i> 1378   </i></font></a>          <font color="#117755"><i>/* <span><a href="1,269">'AttitudeEKF:269'</a></span> P_apo=P_apr; */</i></font>
<a name="1379"><font color="#888888 "><i> 1379   </i></font></a>          memcpy(&amp;<a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[0], &amp;P_apr[0], 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>));
<a name="1380"><font color="#888888 "><i> 1380   </i></font></a>        <b>}</b>
<a name="1381"><font color="#888888 "><i> 1381   </i></font></a>      <b>}</b>
<a name="1382"><font color="#888888 "><i> 1382   </i></font></a>    <b>}</b>
<a name="1383"><font color="#888888 "><i> 1383   </i></font></a>  <b>}</b>
<a name="1384"><font color="#888888 "><i> 1384   </i></font></a>
<a name="1385"><font color="#888888 "><i> 1385   </i></font></a>  <font color="#117755"><i>/* % euler anglels extraction */</i></font>
<a name="1386"><font color="#888888 "><i> 1386   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,278">'AttitudeEKF:278'</a></span> z_n_b = -x_apo(7:9)./norm(x_apo(7:9)); */</i></font>
<a name="1387"><font color="#888888 "><i> 1387   </i></font></a>  maxval = <a href="#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(*(<font color="#112266"><b>float</b></font> (*)[3])&amp;<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[6]);
<a name="1388"><font color="#888888 "><i> 1388   </i></font></a>  a21 = <a href="#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(*(<font color="#112266"><b>float</b></font> (*)[3])&amp;<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[9]);
<a name="1389"><font color="#888888 "><i> 1389   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 3; i++) <b>{</b>
<a name="1390"><font color="#888888 "><i> 1390   </i></font></a>    <font color="#117755"><i>/* <span><a href="1,279">'AttitudeEKF:279'</a></span> m_n_b = x_apo(10:12)./norm(x_apo(10:12)); */</i></font>
<a name="1391"><font color="#888888 "><i> 1391   </i></font></a>    muk[i] = -<a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[i + 6] / maxval;
<a name="1392"><font color="#888888 "><i> 1392   </i></font></a>    zek[i] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[i + 9] / a21;
<a name="1393"><font color="#888888 "><i> 1393   </i></font></a>  <b>}</b>
<a name="1394"><font color="#888888 "><i> 1394   </i></font></a>
<a name="1395"><font color="#888888 "><i> 1395   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,281">'AttitudeEKF:281'</a></span> y_n_b=cross(z_n_b,m_n_b); */</i></font>
<a name="1396"><font color="#888888 "><i> 1396   </i></font></a>  wak[0] = muk[1] * zek[2] - muk[2] * zek[1];
<a name="1397"><font color="#888888 "><i> 1397   </i></font></a>  wak[1] = muk[2] * zek[0] - muk[0] * zek[2];
<a name="1398"><font color="#888888 "><i> 1398   </i></font></a>  wak[2] = muk[0] * zek[1] - muk[1] * zek[0];
<a name="1399"><font color="#888888 "><i> 1399   </i></font></a>
<a name="1400"><font color="#888888 "><i> 1400   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,282">'AttitudeEKF:282'</a></span> y_n_b=y_n_b./norm(y_n_b); */</i></font>
<a name="1401"><font color="#888888 "><i> 1401   </i></font></a>  maxval = <a href="#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(wak);
<a name="1402"><font color="#888888 "><i> 1402   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1403"><font color="#888888 "><i> 1403   </i></font></a>    wak[r2] /= maxval;
<a name="1404"><font color="#888888 "><i> 1404   </i></font></a>  <b>}</b>
<a name="1405"><font color="#888888 "><i> 1405   </i></font></a>
<a name="1406"><font color="#888888 "><i> 1406   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,284">'AttitudeEKF:284'</a></span> x_n_b=(cross(y_n_b,z_n_b)); */</i></font>
<a name="1407"><font color="#888888 "><i> 1407   </i></font></a>  zek[0] = wak[1] * muk[2] - wak[2] * muk[1];
<a name="1408"><font color="#888888 "><i> 1408   </i></font></a>  zek[1] = wak[2] * muk[0] - wak[0] * muk[2];
<a name="1409"><font color="#888888 "><i> 1409   </i></font></a>  zek[2] = wak[0] * muk[1] - wak[1] * muk[0];
<a name="1410"><font color="#888888 "><i> 1410   </i></font></a>
<a name="1411"><font color="#888888 "><i> 1411   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,285">'AttitudeEKF:285'</a></span> x_n_b=x_n_b./norm(x_n_b); */</i></font>
<a name="1412"><font color="#888888 "><i> 1412   </i></font></a>  maxval = <a href="#fcn_norm" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_norm');" target="_self"><font color="#1122aa">norm</font></a>(zek);
<a name="1413"><font color="#888888 "><i> 1413   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1414"><font color="#888888 "><i> 1414   </i></font></a>    zek[r2] /= maxval;
<a name="1415"><font color="#888888 "><i> 1415   </i></font></a>  <b>}</b>
<a name="1416"><font color="#888888 "><i> 1416   </i></font></a>
<a name="1417"><font color="#888888 "><i> 1417   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,288">'AttitudeEKF:288'</a></span> xa_apo=x_apo; */</i></font>
<a name="1418"><font color="#888888 "><i> 1418   </i></font></a>  <font color="#112266"><b>for</b></font> (i = 0; i &lt; 12; i++) <b>{</b>
<a name="1419"><font color="#888888 "><i> 1419   </i></font></a>    xa_apo[i] = <a href="#var_x_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_x_apo');" target="_self"><font color="#1122aa">x_apo</font></a>[i];
<a name="1420"><font color="#888888 "><i> 1420   </i></font></a>  <b>}</b>
<a name="1421"><font color="#888888 "><i> 1421   </i></font></a>
<a name="1422"><font color="#888888 "><i> 1422   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,289">'AttitudeEKF:289'</a></span> Pa_apo=P_apo; */</i></font>
<a name="1423"><font color="#888888 "><i> 1423   </i></font></a>  memcpy(&amp;Pa_apo[0], &amp;<a href="#var_P_apo" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_P_apo');" target="_self"><font color="#1122aa">P_apo</font></a>[0], 144U * <font color="#112266"><b>sizeof</b></font>(<font color="#112266"><b>float</b></font>));
<a name="1424"><font color="#888888 "><i> 1424   </i></font></a>
<a name="1425"><font color="#888888 "><i> 1425   </i></font></a>  <font color="#117755"><i>/*  rotation matrix from earth to body system */</i></font>
<a name="1426"><font color="#888888 "><i> 1426   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,291">'AttitudeEKF:291'</a></span> Rot_matrix=[x_n_b,y_n_b,z_n_b]; */</i></font>
<a name="1427"><font color="#888888 "><i> 1427   </i></font></a>  <font color="#112266"><b>for</b></font> (r2 = 0; r2 &lt; 3; r2++) <b>{</b>
<a name="1428"><font color="#888888 "><i> 1428   </i></font></a>    Rot_matrix[r2] = zek[r2];
<a name="1429"><font color="#888888 "><i> 1429   </i></font></a>    Rot_matrix[3 + r2] = wak[r2];
<a name="1430"><font color="#888888 "><i> 1430   </i></font></a>    Rot_matrix[6 + r2] = muk[r2];
<a name="1431"><font color="#888888 "><i> 1431   </i></font></a>  <b>}</b>
<a name="1432"><font color="#888888 "><i> 1432   </i></font></a>
<a name="1433"><font color="#888888 "><i> 1433   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,294">'AttitudeEKF:294'</a></span> phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */</i></font>
<a name="1434"><font color="#888888 "><i> 1434   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,295">'AttitudeEKF:295'</a></span> theta=-asin(Rot_matrix(1,3)); */</i></font>
<a name="1435"><font color="#888888 "><i> 1435   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,296">'AttitudeEKF:296'</a></span> psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */</i></font>
<a name="1436"><font color="#888888 "><i> 1436   </i></font></a>  <font color="#117755"><i>/* <span><a href="1,297">'AttitudeEKF:297'</a></span> eulerAngles=[phi;theta;psi]; */</i></font>
<a name="1437"><font color="#888888 "><i> 1437   </i></font></a>  eulerAngles[0] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)atan2(Rot_matrix[7], Rot_matrix[8]);
<a name="1438"><font color="#888888 "><i> 1438   </i></font></a>  eulerAngles[1] = -(<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)asin(Rot_matrix[6]);
<a name="1439"><font color="#888888 "><i> 1439   </i></font></a>  eulerAngles[2] = (<a href="rtwtypes_h.html#type_real32_T" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'type_real32_T');" target="_self"><font color="#1122aa">real32_T</font></a>)atan2(Rot_matrix[3], Rot_matrix[0]);
<a name="1440"><font color="#888888 "><i> 1440   </i></font></a><b>}</b>
<a name="1441"><font color="#888888 "><i> 1441   </i></font></a>
<a name="1442"><font color="#888888 "><i> 1442   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="1443"><font color="#888888 "><i> 1443   </i></font></a><font color="#117755"><I> * Arguments    : void</i></font>
<a name="1444"><font color="#888888 "><i> 1444   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="1445"><font color="#888888 "><i> 1445   </i></font></a><font color="#117755"><I> */</i></font>
<a name="1446"><font color="#888888 "><i> 1446   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_AttitudeEKF_initialize">AttitudeEKF_initialize</a>(<font color="#112266"><b>void</b></font>)
<a name="1447"><font color="#888888 "><i> 1447   </i></font></a><b>{</b>
<a name="1448"><font color="#888888 "><i> 1448   </i></font></a>  <a href="#var_Q_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Q_not_empty');" target="_self"><font color="#1122aa">Q_not_empty</font></a> = false;
<a name="1449"><font color="#888888 "><i> 1449   </i></font></a>  <a href="#var_Ji_not_empty" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'var_Ji_not_empty');" target="_self"><font color="#1122aa">Ji_not_empty</font></a> = false;
<a name="1450"><font color="#888888 "><i> 1450   </i></font></a>  <a href="#fcn_AttitudeEKF_init" onclick="if (top) if (top.docHiliteMe) top.docHiliteMe(window, 'fcn_AttitudeEKF_init');" target="_self"><font color="#1122aa">AttitudeEKF_init</font></a>();
<a name="1451"><font color="#888888 "><i> 1451   </i></font></a><b>}</b>
<a name="1452"><font color="#888888 "><i> 1452   </i></font></a>
<a name="1453"><font color="#888888 "><i> 1453   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="1454"><font color="#888888 "><i> 1454   </i></font></a><font color="#117755"><I> * Arguments    : void</i></font>
<a name="1455"><font color="#888888 "><i> 1455   </i></font></a><font color="#117755"><I> * Return Type  : void</i></font>
<a name="1456"><font color="#888888 "><i> 1456   </i></font></a><font color="#117755"><I> */</i></font>
<a name="1457"><font color="#888888 "><i> 1457   </i></font></a><font color="#112266"><b>void</b></font> <a name="fcn_AttitudeEKF_terminate">AttitudeEKF_terminate</a>(<font color="#112266"><b>void</b></font>)
<a name="1458"><font color="#888888 "><i> 1458   </i></font></a><b>{</b>
<a name="1459"><font color="#888888 "><i> 1459   </i></font></a>  <font color="#117755"><i>/* (no terminate code required) */</i></font>
<a name="1460"><font color="#888888 "><i> 1460   </i></font></a><b>}</b>
<a name="1461"><font color="#888888 "><i> 1461   </i></font></a>
<a name="1462"><font color="#888888 "><i> 1462   </i></font></a><font color="#117755"><i>/*</i></font>
<a name="1463"><font color="#888888 "><i> 1463   </i></font></a><font color="#117755"><I> * File trailer for AttitudeEKF.c</i></font>
<a name="1464"><font color="#888888 "><i> 1464   </i></font></a><font color="#117755"><I> *</i></font>
<a name="1465"><font color="#888888 "><i> 1465   </i></font></a><font color="#117755"><I> * [EOF]</i></font>
<a name="1466"><font color="#888888 "><i> 1466   </i></font></a><font color="#117755"><I> */</i></font>
<a name="1467"><font color="#888888 "><i> 1467   </i></font></a></pre>
</td></tr></table>
</p>
</body>
</html>